Merge pull request #103 from frc937/numbers

Numbers

finally lmao

src/main/deploy/swerve/modules/backleft.json

@@ -1,17 +1,17 @@
 {
   "location": {
-    "front": -12,
-    "left": 9
+    "front": -10.875,
+    "left": 10
   },
-  "absoluteEncoderOffset": 211.0,
+  "absoluteEncoderOffset": 56.5,
   "drive": {
     "type": "sparkmax",
-    "id": 5,
+    "id": 8,
     "canbus": null
   },
   "angle": {
     "type": "sparkmax",
-    "id": 6,
+    "id": 4,
     "canbus": null
   },
   "conversionFactor": {
@@ -20,7 +20,7 @@
   },
   "encoder": {
     "type": "thrifty",
-    "id": 2,
+    "id": 1,
     "canbus": null
   },
   "inverted": {

src/main/deploy/swerve/modules/backright.json

@@ -1,17 +1,17 @@
 {
   "location": {
-    "front": -12,
-    "left": -9
+    "front": -10.875,
+    "left": -10
   },
-  "absoluteEncoderOffset": 31.9,
+  "absoluteEncoderOffset": 209.9,
   "drive": {
     "type": "sparkmax",
-    "id": 7,
+    "id": 5,
     "canbus": null
   },
   "angle": {
     "type": "sparkmax",
-    "id": 8,
+    "id": 6,
     "canbus": null
   },
   "conversionFactor": {

src/main/deploy/swerve/modules/frontleft.json

@@ -1,17 +1,17 @@
 {
   "location": {
-    "front": 12,
-    "left": 9
+    "front": 10.875,
+    "left": 10
   },
-  "absoluteEncoderOffset": 140.6,
+  "absoluteEncoderOffset": 260.5,
   "drive": {
     "type": "sparkmax",
-    "id": 3,
+    "id": 9,
     "canbus": null
   },
   "angle": {
     "type": "sparkmax",
-    "id": 4,
+    "id": 10,
     "canbus": null
   },
   "conversionFactor": {

src/main/deploy/swerve/modules/frontright.json

@@ -1,17 +1,17 @@
 {
   "location": {
-    "front": 12,
-    "left": -9
+    "front": 10.875,
+    "left": -10
   },
-  "absoluteEncoderOffset": 5.5,
+  "absoluteEncoderOffset": 1.5,
   "drive": {
     "type": "sparkmax",
-    "id": 1,
+    "id": 3,
     "canbus": null
   },
   "angle": {
     "type": "sparkmax",
-    "id": 2,
+    "id": 7,
     "canbus": null
   },
   "conversionFactor": {
@@ -20,7 +20,7 @@
   },
   "encoder": {
     "type": "thrifty",
-    "id": 1,
+    "id": 2,
     "canbus": null
   },
   "inverted": {

src/main/java/frc/robot/Constants.java

@@ -34,25 +34,25 @@ public static class Mailbox {
   /** Constants for the Pneumatics system. */
   public static class MailboxPneumatics {
     /** The channel on the PCM for the forward direction on the left solenoid. */
-    public static final int LEFT_SOLENOID_FORWARD_CHANNEL = 0;
+    public static final int LEFT_SOLENOID_FORWARD_CHANNEL = 5;
 
     /** The channel on the PCM for the reverse direction on the left solenoid. */
-    public static final int LEFT_SOLENOID_REVERSE_CHANNEL = 1;
+    public static final int LEFT_SOLENOID_REVERSE_CHANNEL = 4;
 
     /** The channel on the PCM for the forward direction on the right solenoid. */
-    public static final int RIGHT_SOLENOID_FORWARD_CHANNEL = 2;
+    public static final int RIGHT_SOLENOID_FORWARD_CHANNEL = 7;
 
     /** The channel on the PCM for the reverse direction on the right solenoid. */
-    public static final int RIGHT_SOLENOID_REVERSE_CHANNEL = 3;
+    public static final int RIGHT_SOLENOID_REVERSE_CHANNEL = 6;
   }
 
   /** Constants for the Belts system. */
   public static class MailboxBelts {
     /** The CAN ID for the upper belt motor */
-    public static final int UPPER_BELT_MOTOR_ID = 0;
+    public static final int UPPER_BELT_MOTOR_ID = 11;
 
     /** The CAN ID for the lower belt motor */
-    public static final int LOWER_BELT_MOTOR_ID = 0;
+    public static final int LOWER_BELT_MOTOR_ID = 12;
 
     /** The speed for the belt motors. */
     public static final double BELT_MOTOR_SPEED = 1;
@@ -62,6 +62,9 @@ public static class MailboxBelts {
 
     /** Inversion state of the belts follower motor. */
     public static final boolean BELTS_FOLLOWER_INVERSE_STATE = false;
+
+    /** Current limit (in amps) for the belt motor(s) */
+    public static final int BELT_MOTOR_CURRENT_LIMIT = 40;
   }
 
   /** Constants that are relating to the controllers. */
@@ -70,7 +73,7 @@ public static class Controllers {
     public static final int PILOT_CONTROLLER_PORT = 0;
 
     /** Driver station port number for the operator controller */
-    public static final int OPERATOR_CONTROLLER_PORT = 1;
+    public static final int OPERATOR_CONTROLLER_PORT = 2;
 
     /** Axis deadband for driver controller. */
     public static double DRIVER_CONTROLLER_DEADBAND = 0.1;
@@ -118,23 +121,26 @@ public static class Auto {
   /** Constants for the Intake System */
   public static class Intake {
     /** Motor id of the Lower Intake motor. */
-    public static final int LOWER_INTAKE_MOTOR_ID = 0;
+    public static final int LOWER_INTAKE_MOTOR_ID = 1;
 
     /** Motor id of the Upper Intake motor. */
-    public static final int UPPER_INTAKE_MOTOR_ID = 0;
+    public static final int UPPER_INTAKE_MOTOR_ID = 2;
 
     /** Inversion state of the upper intake motor. */
     public static final boolean UPPER_INTAKE_MOTOR_INVERTED = false;
 
     // It was me, DIO!
     /** DIO Port ID for the Intake limit switch. */
-    public static final int INTAKE_LIMIT_SWITCH_DIO_PORT = 0;
+    public static final int INTAKE_LIMIT_SWITCH_DIO_PORT = 1;
 
     /** Speed we want to run the Intake at. */
     public static final double INTAKE_MOTOR_SPEED = 1;
 
     /** Inversion state for the intake follower motor. */
     public static final boolean INTAKE_FOLLOWER_INVERSE_STATE = false;
+
+    /** Current limit (in amps) for the intake motor(s) */
+    public static final int INTAKE_MOTOR_CURRENT_LIMIT = 40;
   }
 
   /** Holds contstants for the Limelights. */
@@ -145,39 +151,39 @@ public static class AimingLimelight {
       public static final String LIMELIGHT_NAME = "limelight";
 
       /** The number of degrees the Limelight is mounted back from perfectly vertical */
-      public static final double MOUNT_ANGLE = 0;
+      public static final double MOUNT_ANGLE = 50;
 
       /** The number of inches from the center of the Limelight lens to the floor */
-      public static final double MOUNT_HEIGHT = 0;
+      public static final double MOUNT_HEIGHT = 10.0625;
 
       /** The height to the Amp Apriltag off the ground. */
-      public static final double AMP_APRILTAG_HEIGHT = 0;
+      public static final double AMP_APRILTAG_HEIGHT = 53.13;
 
       /** How far in inches we want to be from the target when we shoot */
-      public static final double DISTANCE_FROM_TARGET = 0;
+      public static final double DISTANCE_FROM_TARGET = 14;
 
       /**
        * How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor
        */
-      public static final double STEER_STRENGTH = 0;
+      public static final double STEER_STRENGTH = 0.01;
 
       /** How hard to drive toward the target. Same notation as above. */
-      public static final double DRIVE_STRENGTH = 0;
+      public static final double DRIVE_STRENGTH = 0.01;
 
       /** VERY BASIC speed limit to make sure we don't drive too fast towards the target. */
-      public static final double SPEED_LIMIT = 0;
+      public static final double SPEED_LIMIT = 0.2;
 
       /**
        * When we're at or below this number of degrees from where we want to be, we'll consider the
        * Limelight's aim routine "done"
        */
-      public static final double TURN_DONE_THRESHOLD = 0;
+      public static final double TURN_DONE_THRESHOLD = 1;
 
       /**
        * When we're at or below this number of inches from the target distance, we'll consider the
        * Limelight's drive routine "done"
        */
-      public static final double DISTANCE_DONE_THRESHOLD = 0;
+      public static final double DISTANCE_DONE_THRESHOLD = 4;
 
       /** Holds pipeline numbers for this Limelight. */
       public static class PipelineNumbers {

src/main/java/frc/robot/Keybinds.java

@@ -29,7 +29,7 @@ public static void configureDefaultKeybinds(
     operatorController.y().whileTrue(RobotContainer.climbUp);
     operatorController.a().whileTrue(RobotContainer.climbDown);
     operatorController.povUp().whileTrue(RobotContainer.runIntake);
-    /* TODO: bind intake reverse to povDown when issue 92 is closed. */
+    operatorController.povDown().whileTrue(RobotContainer.runIntakeReverse);
     operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp);
     operatorController.rightTrigger().whileTrue(RobotContainer.fireNote);
 
@@ -49,7 +49,7 @@ public static void configureOperatorlessKeybinds(
       CommandXboxController pilotController, CommandXboxController operatorController) {
     pilotController.leftStick().toggleOnTrue(RobotContainer.driveFieldOriented);
     /* TODO: angle / velocity steering toggle w/ right stick (no issue) and boost on right bumper (issue 86) */
-    /* TODO: bind intake reverse to povDown when issue 92 is closed. */
+    pilotController.povDown().whileTrue(RobotContainer.runIntakeReverse);
     pilotController.povUp().toggleOnTrue(RobotContainer.enterXMode);
     pilotController.rightTrigger().whileTrue(RobotContainer.runIntake);
     pilotController.a().whileTrue(RobotContainer.aimToAmp);
@@ -70,7 +70,7 @@ public static void configureOriginalKeybinds(
     pilotController.x().onTrue(RobotContainer.enterXMode);
     pilotController.y().whileTrue(RobotContainer.fireNote);
     pilotController.a().whileTrue(RobotContainer.runIntake);
-    /* TODO: bind intake reverse to povDown when issue 92 is closed. */
+    pilotController.povDown().whileTrue(RobotContainer.runIntakeReverse);
     pilotController.b().whileTrue(RobotContainer.aimToAmp);
     pilotController.leftTrigger().whileTrue(RobotContainer.climbDown);
     pilotController.rightTrigger().whileTrue(RobotContainer.climbUp);

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

@@ -178,7 +178,7 @@ public class RobotContainer {
       new FireNoteRoutineNoLimitSwitch();
 
   /** Singleton instance of {@link RunIntakeReverse} for the whole robot. */
-  private RunIntakeReverse runIntakeReverse = new RunIntakeReverse();
+  public static RunIntakeReverse runIntakeReverse = new RunIntakeReverse();
 
   /** Singleton instance of {@link RunIntake} for the whole robot. */
   public static RunIntake runIntake = new RunIntake();
@@ -195,7 +195,8 @@ public class RobotContainer {
           Constants.Limelight.AimingLimelight.SPEED_LIMIT,
           Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD,
           Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD,
-          Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT);
+          Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT,
+          Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER);
 
   /** Singleton instance of {@link AimAndFireRoutine} for the whole robot. */
   public static AimAndFireRoutine aimAndFire = new AimAndFireRoutine();
@@ -253,7 +254,8 @@ private void configureAuto() {
   }
 
   private void configureBindings() {
-    Keybinds.configureOperatorlessKeybinds(pilotController, operatorController);
+    // TODO: CHANGE THIS TO configureDefaultKeybinds() FOR COMP
+    Keybinds.configureDefaultKeybinds(pilotController, operatorController);
   }
 
   /**
@@ -266,9 +268,12 @@ public Command getAutonomousCommand() {
   }
 
   private static double scaleAxis(double axis) {
+    if (axis == 0) {
+      return 0;
+    }
     double deadbanded =
         MathUtil.applyDeadband(axis, Constants.Controllers.DRIVER_CONTROLLER_DEADBAND);
-    return Math.pow(deadbanded, 3);
+    return -Math.pow(deadbanded, 2) * (Math.abs(axis) / axis);
   }
 
   /**

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

@@ -36,7 +36,8 @@ public AimAndFireRoutine() {
             Constants.Limelight.AimingLimelight.SPEED_LIMIT,
             Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD,
             Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD,
-            Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT),
+            Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT,
+            Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER),
         new EnterXMode(),
         new ParallelDeadlineGroup(
             new WaitCommand(Constants.Auto.FIRE_NOTE_FOR_TIME), new FireNoteRoutine()));

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

@@ -33,7 +33,9 @@ public class AimWithLimelight extends Command {
       speedLimit,
       turnDoneThreshold,
       distanceDoneThreshold,
-      targetHeight;
+      targetHeight,
+      pipelineNumber,
+      oldPipelineNumber;
 
   /**
    * Creates a new command to aim with the Limelight.
@@ -61,7 +63,8 @@ public AimWithLimelight(
       double speedLimit,
       double turnDoneThreshold,
       double distanceDoneThreshold,
-      double targetHeight) {
+      double targetHeight,
+      double pipelineNumber) {
     this.steerStrength = steerStrength;
     this.distanceFromTarget = distanceFromTarget;
     this.mountHeight = mountHeight;
@@ -71,6 +74,7 @@ public AimWithLimelight(
     this.turnDoneThreshold = turnDoneThreshold;
     this.distanceDoneThreshold = distanceDoneThreshold;
     this.targetHeight = targetHeight;
+    this.pipelineNumber = pipelineNumber;
     this.drive = RobotContainer.drive;
     this.limelight = limelight;
     // Use addRequirements() here to declare subsystem dependencies.
@@ -80,6 +84,8 @@ public AimWithLimelight(
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    this.oldPipelineNumber = limelight.getLimelightPipeline();
+    limelight.setLimelightPipeline(pipelineNumber);
     this.finished = false;
     this.counter = 0;
   }
@@ -115,7 +121,9 @@ public void execute() {
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    limelight.setLimelightPipeline(oldPipelineNumber);
+  }
 
   // Returns true when the command should end.
   @Override

src/main/java/frc/robot/commands/drive/DriveFieldOriented.java

@@ -40,7 +40,9 @@ public DriveFieldOriented(
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    drive.setHeadingCorrection(false);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

src/main/java/frc/robot/commands/drive/DriveRobotOriented.java

@@ -40,7 +40,9 @@ public DriveRobotOriented(
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    drive.setHeadingCorrection(false);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override