diff --git a/src/main/deploy/pathplanner/autos/Copy of TirSimple.auto b/src/main/deploy/pathplanner/autos/Copy of TirSimple.auto new file mode 100644 index 0000000..c9505de --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of TirSimple.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TirerSimple" + } + }, + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, + { + "type": "named", + "data": { + "name": "Lancer" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DepotTirer.auto b/src/main/deploy/pathplanner/autos/DepotTirer.auto index cc642b6..b6fd578 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirer.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirer.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "Depot" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index 5df75ea..326fc3d 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "Depot" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto index abb0dfb..0f80659 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "Depot" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/MilieuDroite.auto b/src/main/deploy/pathplanner/autos/MilieuDroite.auto index 45028a2..897c60d 100644 --- a/src/main/deploy/pathplanner/autos/MilieuDroite.auto +++ b/src/main/deploy/pathplanner/autos/MilieuDroite.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "ChercherMilieuDroite" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/MilieuDroiteProche.auto b/src/main/deploy/pathplanner/autos/MilieuDroiteProche.auto index 3be46e4..eba39c3 100644 --- a/src/main/deploy/pathplanner/autos/MilieuDroiteProche.auto +++ b/src/main/deploy/pathplanner/autos/MilieuDroiteProche.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "ChercherMilieuDroiteProche" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/MilieuGauche.auto b/src/main/deploy/pathplanner/autos/MilieuGauche.auto index c94ab13..2a9960c 100644 --- a/src/main/deploy/pathplanner/autos/MilieuGauche.auto +++ b/src/main/deploy/pathplanner/autos/MilieuGauche.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "ChercherMilieuGauche" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/MilieuGaucheProche.auto b/src/main/deploy/pathplanner/autos/MilieuGaucheProche.auto index a5f7b5e..d1f496f 100644 --- a/src/main/deploy/pathplanner/autos/MilieuGaucheProche.auto +++ b/src/main/deploy/pathplanner/autos/MilieuGaucheProche.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "deadline", "data": { @@ -14,12 +20,6 @@ "pathName": "ChercherMilieuGaucheProche" } }, - { - "type": "named", - "data": { - "name": "DescendreBalayeuse" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/TirSimple.auto b/src/main/deploy/pathplanner/autos/TirSimple.auto index 48c2566..06339c3 100644 --- a/src/main/deploy/pathplanner/autos/TirSimple.auto +++ b/src/main/deploy/pathplanner/autos/TirSimple.auto @@ -10,6 +10,12 @@ "pathName": "TirerSimple" } }, + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 3e5f363..e3bd2ba 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.83, + "robotLength": 0.87, "holonomicMode": true, "pathFolders": [ "Milieu" @@ -13,23 +13,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 54.5, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, + "driveWheelRadius": 0.1, "driveGearing": 5.143, - "maxDriveSpeed": 5.45, + "maxDriveSpeed": 5.374, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.288925, + "flModuleY": 0.269875, + "frModuleX": 0.288925, + "frModuleY": -0.27, + "blModuleX": -0.288925, + "blModuleY": 0.269875, + "brModuleX": -0.288925, + "brModuleY": -0.27, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11141f5..4c90db2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -72,12 +73,9 @@ public class RobotContainer { public RobotContainer() { - autoChooser = AutoBuilder.buildAutoChooser(); - CameraServer.startAutomaticCapture(); - configureBindings(); NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); - NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur)); + NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite()); NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche()); NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain)); @@ -87,6 +85,12 @@ public class RobotContainer { NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain)); NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur)); NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur)); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + CameraServer.startAutomaticCapture(); + + configureBindings(); } private void configureBindings() { @@ -127,5 +131,6 @@ public class RobotContainer { public Command getAutonomousCommand() { return autoChooser.getSelected(); + // return getAutonomousCommand(); } } diff --git a/src/main/java/frc/robot/commands/DescendreBalyeuse.java b/src/main/java/frc/robot/commands/DescendreBalyeuse.java index 0d22cb8..5c83a98 100644 --- a/src/main/java/frc/robot/commands/DescendreBalyeuse.java +++ b/src/main/java/frc/robot/commands/DescendreBalyeuse.java @@ -41,6 +41,6 @@ public class DescendreBalyeuse extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return Math.abs(balayeuse.Distance()) > balayeuse.EncodeurBalayeuse(); } } diff --git a/src/main/java/frc/robot/commands/DitanceLancer.java b/src/main/java/frc/robot/commands/DitanceLancer.java new file mode 100644 index 0000000..582a32f --- /dev/null +++ b/src/main/java/frc/robot/commands/DitanceLancer.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Led; +import frc.robot.subsystems.Limelight3G; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class DitanceLancer extends Command { + private Led led; + private Limelight3G limelight3g; + /** Creates a new DitanceLancer. */ + public DitanceLancer(Limelight3G limelight3g, Led led) { + this.led = led; + this.limelight3g = limelight3g; + addRequirements(led,limelight3g); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 3333d12..ce24cec 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -25,6 +25,7 @@ public class Lancer extends Command { this.timer = new Timer(); this.limeLight3G = new Limelight3G(); addRequirements(lanceur, limeLight3G); + this.temp = 0; // Use addRequirements() here to declare subsystem dependencies. } @@ -48,10 +49,10 @@ public class Lancer extends Command { botx = BotPose[0]; boty = BotPose[1]; } - double vitesse = 0.5; - if(limeLight3G.getV()){ - vitesse = 3100 + 475 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2))) - 2); - System.out.println(vitesse); + double vitesse = 0.5; + if(limeLight3G.getV()){ + vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; + System.out.println(vitesse); double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index becd06b..b4255a2 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -24,6 +24,7 @@ public class Limelighter extends Command { double botx; double boty; double angle; + double calcul; private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -55,7 +56,7 @@ public class Limelighter extends Command { if(angle >180){ angle =- 360; } - double calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 20; + calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; drivetrain.setControl( drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); System.out.println(calcul); diff --git a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index 49740af..7aa8bc3 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -4,35 +4,59 @@ package frc.robot.commands.ModeAuto; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Lanceur; +import frc.robot.subsystems.Limelight3G; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LancerAuto extends Command { Lanceur lanceur; Timer timer; + private PIDController pidController; + Limelight3G limelight3g; + double botx = 0; + double boty = 0; /** Creates a new LancerAuto. */ - public LancerAuto(Lanceur lanceur) { + public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) { this.lanceur = lanceur; timer = new Timer(); - addRequirements(lanceur); + this.limelight3g = limelight3g; + addRequirements(lanceur, limelight3g); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { - timer.start(); + pidController = new PIDController(0.0007, 0,0, 0.001); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - lanceur.Lancer(0.5); - if(timer.get() > 1){ - lanceur.Demeler(0.1); - } + double[] BotPose = new double[6]; + if(limelight3g.getV()){ + BotPose = limelight3g.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + } + double vitesse = 0.5; + if(limelight3g.getV()){ + vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; + System.out.println(vitesse); + + double output = pidController.calculate(lanceur.Vitesse(),vitesse); + lanceur.Lancer(output); + if(lanceur.Vitesse() >= vitesse-800){ + timer.start(); + if(timer.get() >0.5){ + lanceur.Demeler(1); + } + + } + } } // Called once the command ends or is interrupted. @@ -47,7 +71,7 @@ public class LancerAuto extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get() > 3; + return timer.get() > 4; //return false; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java new file mode 100644 index 0000000..970ae28 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -0,0 +1,91 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.ModeAuto; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Limelight3G; +import static edu.wpi.first.units.Units.*; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class LimelighterAuto extends Command { + Timer timer; + Limelight3G limelight3g; + CommandSwerveDrivetrain drivetrain; + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + double botx; + double boty; + double angle; + double calcul; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + /** Creates a new Limelighter. */ + public LimelighterAuto(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) { + this.limelight3g = limelight3g; + this.drivetrain = drivetrain; + addRequirements(drivetrain, limelight3g); + timer = new Timer(); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double[] BotPose = new double[6]; + if (limelight3g.getV()) { + BotPose = limelight3g.getBotPoseBlue(); + botx = BotPose[1]; + boty = BotPose[0]; + angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + if(angle >180){ + angle =- 360; + } + calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; + drivetrain.setControl( + drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); + System.out.println(calcul); + if (calcul < 0.2 && calcul > -0.2) { + // timer.start(); + // } else { + // timer.reset(); + // } + // } else { + // timer.stop(); + drivetrain.setControl(drive.withRotationalRate(0)); + } + } + else{ + drivetrain.setControl(drive.withRotationalRate(0)); + } + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withRotationalRate(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return calcul < 0.1 && calcul > -0.1; + } +} diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index f99e1c1..2230f29 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -4,11 +4,14 @@ package frc.robot.subsystems; +import java.util.Optional; + import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.RainbowAnimation; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;