hehe

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

src/main/deploy/pathplanner/paths/BlueBasPorter2.path

@@ -9,7 +9,7 @@
       "prevControl": null,
       "nextControl": {
         "x": 3.392520491803279,
-        "y": 1.7293545081967203
+        "y": 1.7293545081967205
       },
       "isLocked": false,
       "linkedName": null

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

@@ -14,15 +14,19 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 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.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 
 import frc.robot.TunerConstants.TunerConstants;
+import frc.robot.commands.RainBow;
+import frc.robot.subsystems.Bougie;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Grimpeur;
 
@@ -38,12 +42,13 @@ public class RobotContainer {
 
     private final Telemetry logger = new Telemetry(MaxSpeed);
 
-    private final CommandXboxController joystick = new CommandXboxController(0);
+    private final CommandXboxController manette1 = new CommandXboxController(0);
+    private final CommandXboxController manette2 = new CommandXboxController(0);
 
     public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
     
     private final SendableChooser<Command> autoChooser;
-    
+    Bougie bougie = new Bougie();
     
     
     public RobotContainer() {
@@ -58,26 +63,31 @@ public class RobotContainer {
         drivetrain.setDefaultCommand(
             // Drivetrain will execute this command periodically
             drivetrain.applyRequest(() ->
-                drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
+                drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward)
-                    .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
+                    .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left)
-                    .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left)
+                    .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left)
             )
         );
 
-        // Run SysId routines when holding back/start and X/Y.
-        // Note that each routine should be run exactly once in a single log.
-        joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
-        joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
-        joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
-        joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
 
         // reset the field-centric heading on left bumper press
-        joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
+        manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
 
         drivetrain.registerTelemetry(logger::telemeterize);
     }
 
+   
         public Command getAutonomousCommand() {
-        return autoChooser.getSelected();
+            return 
+             new 
+            ParallelCommandGroup(
+              autoChooser
+                     .getSelected(),
+            new 
+            RainBow
+            (
+                bougie
+                )
+            );
          }
 }

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

@@ -25,7 +25,7 @@ public class TunerConstants {
     // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
     private static final Slot0Configs steerGains = new Slot0Configs()
         .withKP(100).withKI(0).withKD(0.5)
-        .withKS(0.1).withKV(1.91).withKA(0)
+        .withKS(0.1).withKV(2.66).withKA(0)
         .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
@@ -78,10 +78,10 @@ public class TunerConstants {
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot
-    private static final double kCoupleRatio = 0;
+    private static final double kCoupleRatio = 3.5714285714285716;
 
     private static final double kDriveGearRatio = 6.122448979591837;
-    private static final double kSteerGearRatio = 15.42857142857143;
+    private static final double kSteerGearRatio = 21.428571428571427;
     private static final Distance kWheelRadius = Inches.of(2);
 
     private static final boolean kInvertLeftSide = false;
@@ -126,48 +126,48 @@ public class TunerConstants {
 
 
     // Front Left
-    private static final int kFrontLeftDriveMotorId = 2;
+    private static final int kFrontLeftDriveMotorId = 4;
-    private static final int kFrontLeftSteerMotorId = 6;
+    private static final int kFrontLeftSteerMotorId = 5;
-    private static final int kFrontLeftEncoderId = 9;
+    private static final int kFrontLeftEncoderId = 12;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.046142578125);
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.353271484375);
     private static final boolean kFrontLeftSteerMotorInverted = true;
     private static final boolean kFrontLeftEncoderInverted = false;
 
-    private static final Distance kFrontLeftXPos = Inches.of(13.75);
+    private static final Distance kFrontLeftXPos = Inches.of(13.5);
-    private static final Distance kFrontLeftYPos = Inches.of(13.75);
+    private static final Distance kFrontLeftYPos = Inches.of(10.5);
 
     // Front Right
-    private static final int kFrontRightDriveMotorId = 4;
+    private static final int kFrontRightDriveMotorId = 2;
-    private static final int kFrontRightSteerMotorId = 5;
+    private static final int kFrontRightSteerMotorId = 6;
-    private static final int kFrontRightEncoderId = 12;
+    private static final int kFrontRightEncoderId = 9;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.116455078125);
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2119140625);
     private static final boolean kFrontRightSteerMotorInverted = true;
     private static final boolean kFrontRightEncoderInverted = false;
 
-    private static final Distance kFrontRightXPos = Inches.of(13.75);
+    private static final Distance kFrontRightXPos = Inches.of(13.5);
-    private static final Distance kFrontRightYPos = Inches.of(-13.75);
+    private static final Distance kFrontRightYPos = Inches.of(-10.5);
 
     // Back Left
-    private static final int kBackLeftDriveMotorId = 3;
+    private static final int kBackLeftDriveMotorId = 18;
-    private static final int kBackLeftSteerMotorId = 7;
+    private static final int kBackLeftSteerMotorId = 8;
-    private static final int kBackLeftEncoderId = 10;
+    private static final int kBackLeftEncoderId = 11;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.046630859375);
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(0.236572265625);
     private static final boolean kBackLeftSteerMotorInverted = true;
     private static final boolean kBackLeftEncoderInverted = false;
 
-    private static final Distance kBackLeftXPos = Inches.of(-13.75);
+    private static final Distance kBackLeftXPos = Inches.of(-13.5);
-    private static final Distance kBackLeftYPos = Inches.of(13.75);
+    private static final Distance kBackLeftYPos = Inches.of(10.5);
 
     // Back Right
-    private static final int kBackRightDriveMotorId = 18;
+    private static final int kBackRightDriveMotorId = 3;
-    private static final int kBackRightSteerMotorId = 8;
+    private static final int kBackRightSteerMotorId = 7;
-    private static final int kBackRightEncoderId = 11;
+    private static final int kBackRightEncoderId = 10;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.016357421875);
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.330078125);
     private static final boolean kBackRightSteerMotorInverted = true;
     private static final boolean kBackRightEncoderInverted = false;
 
-    private static final Distance kBackRightXPos = Inches.of(-13.75);
+    private static final Distance kBackRightXPos = Inches.of(-13.5);
-    private static final Distance kBackRightYPos = Inches.of(-13.75);
+    private static final Distance kBackRightYPos = Inches.of(-10.5);
 
 
     public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =

src/main/java/frc/robot/commands/RainBow.java

@@ -0,0 +1,37 @@
+// 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.Bougie;
+
+/* 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 RainBow extends Command {
+  Bougie bougie;
+  /** Creates a new RainBow. */
+  public RainBow(Bougie bougie) {
+    this.bougie = bougie;
+    addRequirements(bougie);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {bougie.RainBow();}
+
+  // 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) {bougie.RainBowStop();}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,38 @@
+// 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.subsystems;
+
+import com.ctre.phoenix.led.CANdle;
+import com.ctre.phoenix.led.CANdleConfiguration;
+import com.ctre.phoenix.led.RainbowAnimation;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Bougie extends SubsystemBase {
+  CANdle candle = new CANdle(5);
+  CANdleConfiguration config = new CANdleConfiguration();
+  RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 64);
+  /** Creates a new Bougie. */
+  public Bougie() {
+    config.brightnessScalar = 0.5;
+    candle.configAllSettings(config);
+  }
+  public void Rouge() {
+   candle.setLEDs(255, 0, 0);
+  }
+  public void Vert() {
+   candle.setLEDs(0, 255, 0);
+  }
+  public void Bleu() {
+   candle.setLEDs(0, 0, 255);
+  }
+  public void RainBow(){candle.animate(rainbowAnim);}
+  public void RainBowStop(){candle.animate(null);}
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
+