Merge pull request #20 from IgniteRobotics/chore/drivetrain-canrange

Orcasphynx · web-flow · commit ff09a2297c8c · 2026-02-17T20:00:20.000-05:00
Added canrange config to drivetrain subsystem
diff --git a/.gitignore b/.gitignore
@@ -186,3 +186,4 @@ compile_commands.json
 # Eclipse generated file for annotation processors
 .factorypath
 .java-version
+src/main/java/frc/robot/BuildConstants.java
diff --git a/src/main/deploy/pathplanner/autos/Plus Auto.auto b/src/main/deploy/pathplanner/autos/Plus Auto.auto
@@ -31,7 +31,7 @@
       ]
     }
   },
-  "resetOdom": true,
+  "resetOdom": false,
   "folder": "Test Autos",
   "choreoAuto": false
 }
diff --git a/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto b/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto
@@ -19,7 +19,7 @@
       ]
     }
   },
-  "resetOdom": true,
+  "resetOdom": false,
   "folder": "Test Autos",
   "choreoAuto": false
 }
diff --git a/src/main/deploy/pathplanner/autos/Square Auto.auto b/src/main/deploy/pathplanner/autos/Square Auto.auto
@@ -31,7 +31,7 @@
       ]
     }
   },
-  "resetOdom": true,
+  "resetOdom": false,
   "folder": "Test Autos",
   "choreoAuto": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Square 1.path b/src/main/deploy/pathplanner/paths/Square 1.path
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Square 2.path b/src/main/deploy/pathplanner/paths/Square 2.path
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Square 3.path b/src/main/deploy/pathplanner/paths/Square 3.path
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Square 4.path b/src/main/deploy/pathplanner/paths/Square 4.path
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,9 +13,11 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import frc.robot.commands.DriveBySpeed;
 import frc.robot.commands.WheelSlipTest;
 import frc.robot.subsystems.climber.ClimberSubsystem;
 import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.subsystems.drive.DrivePreferences;
 import frc.robot.subsystems.drive.DrivetrainSubsystem;
 import frc.robot.subsystems.indexer.IndexerSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
@@ -72,7 +74,8 @@ private void configureSubsystemDefaultCommands() {
                             * Math.copySign(Math.pow(joystick.getRightX(), 2), joystick.getRightX())
                             * DriveConstants
                                 .MAX_ANGULAR_SPEED) // Drive counterclockwise with negative X (left)
-            ));
+                    .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1)
+                    .withRotationalDeadband(DriveConstants.MAX_ANGULAR_SPEED * 0.1)));
   }
 
   private void configureBindings() {
@@ -94,12 +97,15 @@ private void configureBindings() {
     // Run SysId routines when holding back/start and X/Y.
     // Note that each routine should be run exactly once in a single log.
 
-    joystick.x().onTrue(drivetrain.sysIdSteer());
-    joystick.y().onTrue(drivetrain.sysIdTranslation());
-    joystick.b().whileTrue(new WheelSlipTest(drivetrain)); // Testing only
+    // joystick.x().onTrue(drivetrain.sysIdSteer());
+    // joystick.y().onTrue(drivetrain.sysIdTranslation());
+    joystick.x().onTrue(new WheelSlipTest(drivetrain));
+    joystick
+        .y()
+        .whileTrue(new DriveBySpeed(drivetrain, DrivePreferences.onemeter_speed)); // Testing only
 
     // Reset the field-centric heading on left bumper press.
-    joystick.leftBumper().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
+    joystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
 
     drivetrain.registerTelemetry(logger::telemeterize);
   }
diff --git a/src/main/java/frc/robot/commands/DriveBySpeed.java b/src/main/java/frc/robot/commands/DriveBySpeed.java
@@ -0,0 +1,52 @@
+// 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 com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.CommandSwerveDrivetrain;
+import frc.robot.preferences.DoublePreference;
+
+/* 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 DriveBySpeed extends Command {
+  private DoublePreference m_driveSpeed;
+  private CommandSwerveDrivetrain m_drivetrain;
+
+  private final SwerveRequest.RobotCentric m_driveRequest =
+      new SwerveRequest.RobotCentric()
+          .withDriveRequestType(DriveRequestType.OpenLoopVoltage)
+          .withSteerRequestType(SteerRequestType.MotionMagicExpo);
+
+  public DriveBySpeed(CommandSwerveDrivetrain drivetrain, DoublePreference speed) {
+    m_drivetrain = drivetrain;
+    m_driveSpeed = speed;
+    addRequirements(drivetrain);
+  }
+
+  // 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() {
+    m_drivetrain.setControl(m_driveRequest.withVelocityX(m_driveSpeed.getValue()));
+    ;
+  }
+
+  // Called once the command ends or is .interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_drivetrain.setControl(m_driveRequest.withVelocityX(0).withVelocityX(0).withRotationalRate(0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/WheelSlipTest.java b/src/main/java/frc/robot/commands/WheelSlipTest.java
@@ -19,6 +19,7 @@ public class WheelSlipTest extends Command {
   private DrivetrainSubsystem m_drive;
   private double step;
   private ScheduledExecutorService scheduler = Executors.newSingleThreadScheduledExecutor();
+  private SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
   /** Creates a new WheelSlipTest. */
   public WheelSlipTest(DrivetrainSubsystem drive) {
@@ -28,14 +29,12 @@ public WheelSlipTest(DrivetrainSubsystem drive) {
   }
 
   public void incrementStep() {
-    step += 0.05;
-    m_drive.setDrivePower(step);
+    step += 0.025;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
     m_drive.seedFieldCentric();
     point.withModuleDirection(new Rotation2d(0.0));
@@ -51,7 +50,10 @@ public void initialize() {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    m_drive.setDrivePower(step);
+    point.withModuleDirection(new Rotation2d(0.0));
+  }
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java
@@ -54,7 +54,7 @@ public class TunerConstants {
 
   // The stator current at which the wheels start to slip;
   // This needs to be tuned to your individual robot
-  private static final Current kSlipCurrent = Amps.of(120);
+  private static final Current kSlipCurrent = Amps.of(30);
 
   // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
   // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
@@ -86,7 +86,7 @@ public class TunerConstants {
 
   private static final double kDriveGearRatio = 6.746031746031747;
   private static final double kSteerGearRatio = 21.428571428571427;
-  private static final Distance kWheelRadius = Inches.of(2);
+  private static final Distance kWheelRadius = Inches.of(1.81);
 
   private static final boolean kInvertLeftSide = false;
   private static final boolean kInvertRightSide = true;
@@ -136,7 +136,7 @@ public class TunerConstants {
   private static final int kFrontLeftDriveMotorId = 11;
   private static final int kFrontLeftSteerMotorId = 12;
   private static final int kFrontLeftEncoderId = 13;
-  private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.1220703125);
+  private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.1206);
   private static final boolean kFrontLeftSteerMotorInverted = true;
   private static final boolean kFrontLeftEncoderInverted = false;
 
@@ -147,7 +147,7 @@ public class TunerConstants {
   private static final int kFrontRightDriveMotorId = 21;
   private static final int kFrontRightSteerMotorId = 22;
   private static final int kFrontRightEncoderId = 23;
-  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.298828125);
+  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2983);
   private static final boolean kFrontRightSteerMotorInverted = true;
   private static final boolean kFrontRightEncoderInverted = false;
 
@@ -158,7 +158,7 @@ public class TunerConstants {
   private static final int kBackLeftDriveMotorId = 31;
   private static final int kBackLeftSteerMotorId = 32;
   private static final int kBackLeftEncoderId = 33;
-  private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.331787109375);
+  private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.3320);
   private static final boolean kBackLeftSteerMotorInverted = true;
   private static final boolean kBackLeftEncoderInverted = false;
 
@@ -169,7 +169,7 @@ public class TunerConstants {
   private static final int kBackRightDriveMotorId = 41;
   private static final int kBackRightSteerMotorId = 42;
   private static final int kBackRightEncoderId = 43;
-  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.054931640625);
+  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.0534);
   private static final boolean kBackRightSteerMotorInverted = true;
   private static final boolean kBackRightEncoderInverted = false;
 
diff --git a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java
@@ -2,7 +2,7 @@
 
 import frc.robot.preferences.DoublePreference;
 
-final class DrivePreferences {
+public final class DrivePreferences {
 
   protected static DoublePreference translation_kP =
       new DoublePreference("Drive/Translation/kP", 0);
@@ -11,4 +11,7 @@ final class DrivePreferences {
 
   protected static DoublePreference rotation_kP = new DoublePreference("Drive/Rotation/kP", 0);
   protected static DoublePreference rotation_kD = new DoublePreference("Drive/Rotation/kD", 0);
+
+  public static DoublePreference onemeter_speed =
+      new DoublePreference("Drive/Speed for 1m (command)");
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.drive;
 
+import com.ctre.phoenix6.configs.ProximityParamsConfigs;
+import com.ctre.phoenix6.hardware.CANrange;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -11,6 +13,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.generated.CommandSwerveDrivetrain;
@@ -22,16 +25,22 @@
 public class DrivetrainSubsystem extends CommandSwerveDrivetrain {
   private DriveState driveState = DriveState.getInstance();
 
+  private final CANrange drive_canrange;
+
   public DrivetrainSubsystem() {
     super(
         TunerConstants.DrivetrainConstants,
         TunerConstants.FrontLeft,
         TunerConstants.FrontRight,
         TunerConstants.BackLeft,
         TunerConstants.BackRight);
+
+    drive_canrange = new CANrange(9, "rio");
+
     applySteerGains();
     applyDriveGains();
     configureAutoBuilder();
+    configureCANrange();
   }
 
   @Override
@@ -45,6 +54,9 @@ public void periodic() {
           estimate.getTrust());
     }
     driveState.adjustCurrentDriveStats(this.getStateCopy());
+
+    SmartDashboard.putNumber(
+        "Drive Canrange Distance", drive_canrange.getDistance(true).getValueAsDouble());
   }
 
   public Command sysIdSteer() {
@@ -178,6 +190,11 @@ public PIDController getRotationPIDController() {
     return controller;
   }
 
+  public void configureCANrange() {
+    ProximityParamsConfigs proximityParamsConfigs = new ProximityParamsConfigs();
+    drive_canrange.getConfigurator().refresh(proximityParamsConfigs);
+  }
+
   private void configureAutoBuilder() {
     try {
       var config = RobotConfig.fromGUISettings();

Original file line number	Diff line number	Diff line change
`@@ -31,7 +31,7 @@`
`31`	`31`	`]`
`32`	`32`	`}`
`33`	`33`	`},`
`34`		`- "resetOdom": true,`
	`34`	`+ "resetOdom": false,`
`35`	`35`	`"folder": "Test Autos",`
`36`	`36`	`"choreoAuto": false`
`37`	`37`	`}`
Original file line number	Diff line number	Diff line change
`@@ -19,7 +19,7 @@`
`19`	`19`	`]`
`20`	`20`	`}`
`21`	`21`	`},`
`22`		`- "resetOdom": true,`
	`22`	`+ "resetOdom": false,`
`23`	`23`	`"folder": "Test Autos",`
`24`	`24`	`"choreoAuto": false`
`25`	`25`	`}`