Swerve calibration

src/main/java/frc4388/robot/Constants.java

@@ -113,7 +113,7 @@ public final class Constants {
 
         private static final class ModuleSpecificConstants { //2025
             //Front Left
-            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.22705078125+0.5);
+            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
             private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -121,7 +121,7 @@ public final class Constants {
             private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
             
             //Front Right
-            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.07666015625);
+            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
             private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -129,7 +129,7 @@ public final class Constants {
             private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
 
             //Back Left
-            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.37646484375);
+            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
             private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -137,7 +137,7 @@ public final class Constants {
             private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
             
             //Back Right
-            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.219970703125+0.5);
+            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
             private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
@@ -287,8 +287,12 @@ public final class Constants {
       }
     
     public static final class LiDARConstants {
+        public static final int REEF_LIDAR_DIO_CHANNEL = 7;
+        public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
+
+        public static final int HUMAN_PLAYER_STATION_DISTANCE = 25;
+
         public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
-        public static final int LIDAR_DIO_CHANNEL = 7;
         public static final int LIDAR_MICROS_TO_CM = 10;
         public static final int SECONDS_TO_MICROS = 1000000;
     }
@@ -331,7 +335,7 @@ public final class Constants {
         // public static final int L3_DRIVE_TIME = 500;
         public static final int L2_DRIVE_TIME = 250; //Milliseconds
         public static final int ALGAE_DRIVE_TIME = 500;
-        public static final double STOP_VELOCITY = 0.25;
+        public static final double STOP_VELOCITY = 0.0;
     }
 
     public static final class VisionConstants { 
@@ -394,8 +398,8 @@ public final class Constants {
         public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
 
-        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 0;
+        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
-        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 999;
+        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
 
         // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
         

src/main/java/frc4388/robot/RobotContainer.java

@@ -28,6 +28,7 @@ import frc4388.utility.controller.XboxController;
 import frc4388.utility.controller.ButtonBox;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.robot.Constants.FieldConstants;
+import frc4388.robot.Constants.LiDARConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.AutoConstants;
@@ -44,6 +45,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 // Autos
 import frc4388.utility.controller.VirtualController;
+import frc4388.robot.commands.DriveUntilLiDAR;
 import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
@@ -91,7 +93,8 @@ public class RobotContainer {
     /* Subsystems */
     public final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
-    public final Lidar m_lidar = new Lidar();
+    public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
+    public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
     public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -143,7 +146,7 @@ public class RobotContainer {
         new waitElevatorRefrence(m_robotElevator),
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
 
@@ -178,7 +181,7 @@ public class RobotContainer {
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
 
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
 
@@ -211,7 +214,7 @@ public class RobotContainer {
         new waitElevatorRefrence(m_robotElevator),
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -238,7 +241,7 @@ public class RobotContainer {
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
         
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
 
@@ -255,7 +258,7 @@ public class RobotContainer {
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT),
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
         
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -271,7 +274,7 @@ public class RobotContainer {
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT),
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
 
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -287,7 +290,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -301,7 +304,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -339,7 +342,8 @@ public class RobotContainer {
         NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy());
         NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
 
-
+        NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, 
+        new Translation2d(0,1), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, false));
         NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
 
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
@@ -478,11 +482,14 @@ public class RobotContainer {
                 ), m_robotSwerveDrive))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar));
+
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(thrustIntake.asProxy());
           
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
+            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar)); 
         
             
         // ?  /* Operator Buttons */

src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java

@@ -0,0 +1,50 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.TimesNegativeOne;
+
+// Command to repeat a joystick movement for a specific time.
+public class DriveUntilLiDAR extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final Lidar m_lidar;
+    private final double mindistance;
+    private final boolean robotRelative;
+
+    public DriveUntilLiDAR(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        Lidar lidar,
+        double mindistance,
+        boolean robotRelative) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.m_lidar = lidar;
+        this.mindistance = mindistance;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveFine(leftStick, rightStick, 0.1);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(m_lidar.getDistance()) < mindistance;
+    }
+}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -302,7 +302,7 @@ public class Elevator extends Subsystem {
 
 
     if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
-      PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble());
+      PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
     }
 
     // This method will be called once per scheduler run

src/main/java/frc4388/robot/subsystems/Lidar.java

@@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel;
 // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
 public class Lidar extends Subsystem {
 
-    private double distance = -1;
+    private Counter LidarPWM;
-    Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
+    private String name = "Lidar";
 
-    public Lidar() {
+    private double distance = -1;
+    public Lidar(int port, String name) {
+        this.name = name;
+        LidarPWM = new Counter(port);
         LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
         LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
         LidarPWM.reset();
+
+        
+    subsystemLayout = Shuffleboard.getTab("Subsystems")
+    .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+    .withSize(2, 2);
+
+    sbDistance = subsystemLayout
+    .add("Distance", 0)
+    .withWidget(BuiltInWidgets.kGraph)
+    .getEntry();
+
+    sbWithinDistance = subsystemLayout
+    .   add("Within Distance", 0)
+    .withWidget(BuiltInWidgets.kBooleanBox)
+    .getEntry();
     }
 
     @Override
@@ -41,23 +59,13 @@ public class Lidar extends Subsystem {
         return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
     }
 
-    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+    ShuffleboardLayout subsystemLayout;
-            .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+    GenericEntry sbDistance;
-            .withSize(2, 2);
+    GenericEntry sbWithinDistance;
-
-    GenericEntry sbDistance = subsystemLayout
-            .add("Distance", 0)
-            .withWidget(BuiltInWidgets.kGraph)
-            .getEntry();
-
-    GenericEntry sbWithinDistance = subsystemLayout
-            .add("Within Distance", 0)
-            .withWidget(BuiltInWidgets.kBooleanBox)
-            .getEntry();
 
     @Override
     public String getSubsystemName() {
-        return "Lidar";
+        return "Lidar " + name;
     }
 
     @Override

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -237,11 +237,11 @@ public class SwerveDrive extends Subsystem {
             .withVelocityX(leftStick.getX() * speedAdjust)
             .withVelocityY(leftStick.getY() * speedAdjust)
             .withTargetDirection(heading);
-        ctrl.HeadingController.setPID(
+        // ctrl.HeadingController.setPID(
-            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
-            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
-            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
-        );
+        // );
         swerveDriveTrain.setControl(ctrl);
     }
 
@@ -349,7 +349,7 @@ public class SwerveDrive extends Subsystem {
                 rotTarget += deltaAngle;
             }
             
-            swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency());
+            swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
             //back to the ~~future~~ *past*
         }