led

src/main/java/frc/robot/RobotContainer.java

@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -60,15 +61,16 @@ import frc.robot.commands.Elevateur.balonL3;
 public class RobotContainer {
   ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
     .withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
-  GenericEntry L1 = layoutauto.add("choix L1",true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
-  GenericEntry sortirAngle = layoutauto.add("Cote?",true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
-  GenericEntry Reculer = layoutauto.add("Reculer",true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
-  GenericEntry L4 = layoutauto.add("L4",true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry sortirAngle = layoutauto.add("Cote?",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry ReculerB = layoutauto.add("ReculerB",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry ReculerR = layoutauto.add("ReculerR",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry L4 = layoutauto.add("L4",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
 
   /* Setting up bindings for necessary control of the swerve drive platform */
-  private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric()
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
     .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
     .withDriveRequestType(DriveRequestType.OpenLoopVoltage
     ); // Use open-loop control for drive motors
@@ -78,7 +80,6 @@ public class RobotContainer {
   private final CommandXboxController manette2 = new CommandXboxController(1);
   private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
   public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-  //private final SendableChooser<Command> autoChooser;
   public double getAngle() {
     return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
   }
@@ -141,14 +142,14 @@ public class RobotContainer {
     manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
     manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
     
-     //Pince manuel
-     pince.setDefaultCommand(new RunCommand(()->{
-      pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
+    //Pince manuel
+    pince.setDefaultCommand(new RunCommand(()->{
+    pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
     }, pince));
 
     //Elevateur manuel
     elevateur.setDefaultCommand(new RunCommand(()->{
-      elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
+    elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
     }, elevateur));
 
     //Reset encodeur
@@ -158,31 +159,30 @@ public class RobotContainer {
     public Command getAutonomousCommand() {
       return new SequentialCommandGroup(
         drivetrain.applyRequest(()->
-        drive.withVelocityX(-0.1*MaxSpeed)
+        drive.withVelocityX(0.1*MaxSpeed)
        .withVelocityY(0)
-       .withRotationalRate(0)).unless(()->!Reculer.getBoolean(true)).withTimeout(2),
-      drivetrain.applyRequest(()->
-      drive.withVelocityX(0.5*MaxSpeed)
-     .withVelocityY(0)
-     .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2),
+       .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
+       drivetrain.applyRequest(()->
+       drive.withVelocityX(-0.1*MaxSpeed)
+      .withVelocityY(0)
+      .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
+        drivetrain.applyRequest(()->
+         drive.withVelocityX(-0.1*MaxSpeed)
+        .withVelocityY(0)
+        .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
      drivetrain.applyRequest(()->
-     drive.withVelocityX(-0.5*MaxSpeed)
+     drive.withVelocityX(0.1*MaxSpeed)
     .withVelocityY(0)
-    .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)).withTimeout(2),
+    .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
+    drivetrain.applyRequest(()->
+    drive.withVelocityX(-0.1*MaxSpeed)
+   .withVelocityY(0)
+   .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
        drivetrain.applyRequest(()->
         drive.withVelocityX(0)
        .withVelocityY(0)
        .withRotationalRate(0)).withTimeout(0.1),
         new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
-        drivetrain.applyRequest(()->
-        drive.withVelocityX(0.2*MaxSpeed)
-       .withVelocityY(0)
-       .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(0.5),
-       drivetrain.applyRequest(()->
-       drive.withVelocityX(0*MaxSpeed)
-      .withVelocityY(0)
-      .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2),
-        new CoralExpire(pince, bougie).unless(()->!L4.getBoolean(true)).withTimeout(2),
         new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
         new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
         new RainBow(bougie));

src/main/java/frc/robot/TunerConstants/TunerConstants.java

@@ -78,7 +78,7 @@ public class TunerConstants {
 
     // Theoretical free speed (m/s) at 12 V applied output;
     // This needs to be tuned to your individual robot
-    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21);
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6);
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot

src/main/java/frc/robot/commands/Elevateur/L2.java

@@ -26,10 +26,10 @@ public class L2 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-     if(pince.encodeurpivot()>=14 && pince.encodeurpivot()<=15){
+     if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){
        pince.pivote(0);
      }
-     else if(pince.encodeurpivot()>=14){
+     else if(pince.encodeurpivot()>=15){
        pince.pivote(-0.2);
      }
      else{

src/main/java/frc/robot/commands/Elevateur/StationPince.java

@@ -38,13 +38,13 @@ public class StationPince extends Command {
     if(pince.emperagecoral() >= 18){     
       pince.x = true;
     }
-    if(elevateur.position()<=-0.1 && elevateur.position()>= -0.2){
+      if(elevateur.position()<=-0.4 && elevateur.position()>= -0.5){
       elevateur.vitesse(0);
     }
-    else if(elevateur.position()>=-0.1){
+        else if(elevateur.position()>=-0.4){
       elevateur.vitesse(-0.7);
       }
-      else{
+        else{
         elevateur.vitesse(0.25);
       }
     if(pince.x){
@@ -55,7 +55,7 @@ public class StationPince extends Command {
         pince.pivote(0);
       }
     }
-    else{
+      else{
       pince.aspirecoral(0.25);
       
     }

src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java

@@ -27,8 +27,8 @@ public class BalayeuseAlgue extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double cibleMin = 700;
-    double cibleMax = 900;
+    double cibleMin = 550;
+    double cibleMax = 650;
 
      if(requin.amp()>=78.2){
        requin.xRequin = true;

src/main/java/frc/robot/commands/requin/ExpireCorail.java

@@ -28,12 +28,12 @@ public class ExpireCorail extends Command {
   @Override
   public void execute() {
     if(requin.amp()> 60){
-      requin.balaye(-0.4);
+      requin.balaye(-0.1);
     }
     else
     {
     bougie.Rouge();
-    requin.balaye(-0.4);   
+    requin.balaye(-0.1);   
   }
 }
 

src/main/java/frc/robot/commands/requin/L1Requin.java

@@ -27,10 +27,10 @@ public class L1Requin extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(requin.encodeur()<=530 &&  requin.encodeur()>=430){
+    if(requin.encodeur()<=485 &&  requin.encodeur()>=385){
       requin.rotationer(0);
     }
-    else if(requin.encodeur()>=530){
+    else if(requin.encodeur()>=485){
     requin.rotationer(-0.5);
     }
     else{

src/main/java/frc/robot/commands/requin/exspire.java

@@ -31,11 +31,11 @@ public class exspire extends Command {
   public void execute() {
    if(requin.amp()> 15)
    {
-    requin.balaye(0.4);
+    requin.balaye(0.2);
    }
    else{
     bougie.Rouge();
-    requin.balaye(0.4);
+    requin.balaye(0.2);
    }
   }
 

src/main/java/frc/robot/subsystems/Bougie.java

@@ -6,14 +6,22 @@ package frc.robot.subsystems;
 
 import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix.led.CANdleConfiguration;
+import com.ctre.phoenix.led.FireAnimation;
+import com.ctre.phoenix.led.LarsonAnimation;
 import com.ctre.phoenix.led.RainbowAnimation;
+import com.ctre.phoenix.led.TwinkleAnimation;
+import com.ctre.phoenix.led.TwinkleOffAnimation;
+import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent;
+import com.ctre.phoenix.led.LarsonAnimation.BounceMode;
+import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Bougie extends SubsystemBase {
   CANdle candle = new CANdle(23);
   CANdleConfiguration config = new CANdleConfiguration();
-  RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 68);
+  LarsonAnimation rainbowAnim = new LarsonAnimation(256,0,0,0,0.1,68,BounceMode.Front,10,8);
+  //TwinkleOffAnimation rainbowAnim = new TwinkleOffAnimation(256, 0, 0,0,0.5,68,TwinkleOffPercent.Percent88,8);
   /** Creates a new Bougie. */
   public Bougie() {
     config.brightnessScalar = 0.5;

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -45,42 +45,42 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
     private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
 
-    private void configureAutoBuilder() {
-        try {
-            var config = RobotConfig.fromGUISettings();
-            AutoBuilder.configure(
+    // private void configureAutoBuilder() {
+    //     try {
+    //         var config = RobotConfig.fromGUISettings();
+    //         AutoBuilder.configure(
                 
-                () -> getState().Pose,   // Supplier of current robot pose
-                this::resetPose,         // Consumer for seeding pose against auto
-                () -> getState().Speeds, // Supplier of current robot speeds
+    //             () -> getState().Pose,   // Supplier of current robot pose
+    //             this::resetPose,         // Consumer for seeding pose against auto
+    //             () -> getState().Speeds, // Supplier of current robot speeds
                 
-                // Consumer of ChassisSpeeds and feedforwards to drive the robot
-                (speeds, feedforwards) -> setControl(
-                    m_pathApplyRobotSpeeds.withSpeeds(speeds)
-                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
-                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
-                ),
-                new PPHolonomicDriveController(
-                    // PID constants for translation
-                    new PIDConstants(63.167, 0, 0.54521),
-                    // // PID constants for rotation
-                    // new PIDConstants(7.9735, 0, 0.038499)
-                    // PID constants for rotation
-                    new PIDConstants(43.502,0,2.7353)
-                ),
-                config,
-                // Assume the path needs to be flipped for Red vs Blue, this is normally the case
-                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
-                this // Subsystem for requirements
+    //             // Consumer of ChassisSpeeds and feedforwards to drive the robot
+    //             (speeds, feedforwards) -> setControl(
+    //                 m_pathApplyRobotSpeeds.withSpeeds(speeds)
+    //                     .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
+    //                     .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
+    //             ),
+    //             new PPHolonomicDriveController(
+    //                 // PID constants for translation
+    //                 new PIDConstants(63.167, 0, 0.54521),
+    //                 // // PID constants for rotation
+    //                 // new PIDConstants(7.9735, 0, 0.038499)
+    //                 // PID constants for rotation
+    //                 new PIDConstants(43.502,0,2.7353)
+    //             ),
+    //             config,
+    //             // Assume the path needs to be flipped for Red vs Blue, this is normally the case
+    //             () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+    //             this // Subsystem for requirements
                 
-            );
-        } catch (Exception ex) {
-            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
-        }
-         PPHolonomicDriveController.overrideRotationFeedback(()->{
-            return 0;
-         });  
-        }
+    //         );
+    //     } catch (Exception ex) {
+    //         DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+    //     }
+    //      PPHolonomicDriveController.overrideRotationFeedback(()->{
+    //         return 0;
+    //      });  
+    //     }
 
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
@@ -100,7 +100,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-        configureAutoBuilder();
+        //configureAutoBuilder();
     }
 
     /**
@@ -125,7 +125,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-        configureAutoBuilder();
+     //   configureAutoBuilder();
     }
 
     /**
@@ -158,7 +158,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-        configureAutoBuilder();
+        //configureAutoBuilder();
     }