From aa4719445c778657694316edf4e27af55a848a21 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Tue, 3 Mar 2026 23:16:06 -0800 Subject: [PATCH 01/14] started cleaning up constants.java --- .../java/org/carlmontrobotics/Constants.java | 150 +++++++----------- 1 file changed, 54 insertions(+), 96 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index bdad6ca..afd896e 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -64,38 +64,71 @@ public static final class Manipulator { //#region Drivetrain public static final class Drivetrainc { - public static final double wheelBase = Units.inchesToMeters(16.750003); // Correct measurments for hammerhead - public static final double trackWidth = Units.inchesToMeters(23.750000); // Correct measurments for hammerhead - // "swerveRadius" is the distance from the center of the robot to one of the modules + //general drivetrain constants + public static final int driveFrontLeftPort = 1; + public static final int driveFrontRightPort = 2; + public static final int driveBackLeftPort = 3; + public static final int driveBackRightPort = 4; + + public static final int turnFrontLeftPort = 11; + public static final int turnFrontRightPort = 12; + public static final int turnBackLeftPort = 13; + public static final int turnBackRightPort = 14; + //TODO: set hammerhead can coder ports the same as kitbot + public static final int canCoderPortFL = CONFIG.isHammerHead() ? 0 : 1; + public static final int canCoderPortFR = CONFIG.isHammerHead() ? 1 : 2; + public static final int canCoderPortBL = CONFIG.isHammerHead() ? 3 : 3; + public static final int canCoderPortBR = CONFIG.isHammerHead() ? 2 : 0; + + // swerve config constants + public static final double wheelBase = CONFIG.isHammerHead() ? Units.inchesToMeters(16.750003) : + Units.inchesToMeters(16.750003); + public static final double trackWidth = CONFIG.isHammerHead() ? Units.inchesToMeters(23.750000) : + Units.inchesToMeters(23.750000); public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); - // The gearing reduction from the drive motor controller to the wheels - // Gearing for the Swerve Modules is 6.75 : 1 + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65; public static final double driveGearing = 6.75; - // Turn motor shaft to "module shaft" - public static final double turnGearing = 150.0 / 7; + public static final double mu = 1; /* 70/83.2; */ // coefficient of friction. less means less max acceleration. + public static final double autoCentripetalAccel = mu * g * 2; + public static final double[] kForwardVolts = CONFIG.isHammerHead() ? new double[] { 0.2,0.2,0.2,0.2 }: //kS + new double[] {0, 0, 0, 0}; + public static final double[] kForwardVels = CONFIG.isHammerHead() ? new double[] { 0,0,0,0 } : //kV + new double[] { 2.9875, 2.9875, 2.7323, 2.9264 }; + public static final double[] kForwardAccels = { 0, 0, 0, 0 };//{0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 + public static final double[] kBackwardVolts = kForwardVolts; + public static final double[] kBackwardVels = kForwardVels; + public static final double[] kBackwardAccels = kForwardAccels; + public static final double[] drivekP = {1, 1, 1, 1}; + public static final double[] drivekI = {0, 0, 0, 0}; + public static final double[] drivekD = CONFIG.isHammerHead()? new double[] { 0, 0, 0, 0 }: + new double[] { 0,0,0,0 }; + public static final double[] turnkP = CONFIG.isHammerHead() ? new double[] {50, 50, 50, 50} : + new double[] {0,0,0,0}; + public static final double[] turnkI = {0, 0, 0, 0}; + public static final double[] turnkD = {0, 0, 0, 0}; + public static final double[] turnkS = {1, 1, 1, 1}; + public static final double[] turnkV = {0, 0, 0, 0}; + public static final double[] turnkA = {0, 0, 0, 0}; + public static final double[] turnZeroDeg = CONFIG.isHammerHead() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } : + new double[] { 17.2266, -96.8555, -95.8008, 85.166 }; + public static final boolean[] driveInversion = CONFIG.isHammerHead() ? new boolean[] { true, false, true, false } : + new boolean[] { false, true, false, true }; + public static final boolean[] reversed = { false, false, false, false }; public static final double driveModifier = 1; - public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65; // empiric correction FIXME this should be fine tuned - - public static final double mu = 1; /* 70/83.2; */ // coefficient of friction. less means less max acceleration. - public static final double ROBOTMASS_KG = 36.2874;// 80lb ish - // moment of inertia, kg/mm - // calculated by integral of mass * radius^2 for every point of the robot - // easy way? just do total mass * radius^2 - // This seems to be relying that all the mass is located in the corners - //public static final double MOI = ROBOTMASS_KG * swerveRadius * swerveRadius; caveman way - // USE ONSHAPE it has a calculator for this - public static final double MOI = Math.pow(Units.inchesToMeters(1),2) * Units.lbsToKilograms(1) * 14040.21738; // 14040.21738 in^2 lb + public static final boolean[] turnInversion = { true, true, true, true }; + public static final double turnGearing = 150.0 / 7; + public static final double ROBOTMASS_KG = CONFIG.isHammerHead() ? 35.49159484 : 48.582; + public static final double MOI = CONFIG.isHammerHead() ? 4.10872647 : 5.38619461; // moment of inertia, kg/m^2, Lzz in onshape public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s public static final double VortexFreeSpeed = 6784 * (2 * Math.PI) / 60; // radians/s // Angular speed to translational speed --> v = omega * r / gearing public static final double maxSpeed = (CONFIG.isVortexDrive() ? VortexFreeSpeed : NEOFreeSpeed) * (wheelDiameterMeters / 2.0) / driveGearing; // meter/s - public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out - public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxForward = maxSpeed; + public static final double maxStrafe = maxSpeed; // seconds it takes to go from 0 to 12 volts(aka MAX) public static final double secsPer12Volts = 0.1; - // maxRCW is the angular velocity of the robot. // Calculated by looking at one of the motors and treating it as a point mass // moving around in a circle. @@ -104,73 +137,12 @@ public static final class Drivetrainc { // Angular velocity = Tangential speed / radius public static final double maxRCW = maxSpeed / swerveRadius; - public static final boolean[] reversed = { false, false, false, false }; - // public static final boolean[] reversed = {true, true, true, true}; - // Determine correct turnZero constants (FL, FR, BL, BR) - public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } - : (CONFIG.isHammerHead() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } - : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* real values here */ - - // kP, kI, and kD constants for turn motor controllers in the order of - // front-left, front-right, back-left, back-right. - // Determine correct turn PID constants - public static final double[] turnkP = CONFIG.isHammerHead() ? new double[] {0,0,0,0} : - new double[]{0,0,0,0};//{1.9085, /*0.21577*/0.1, /*0.12356*/0.05, 0.36431};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, - // 0.00374}; - public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; - public static final double[] turnkD = { 0, 0, 0, 0 };// : - //new double[]{0, 0, 0, 0};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d - // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = new double[]{ 1, 1, 1, 1}; - //new double[]{0.21969, 0.11487, 0.18525, 0.24865};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; - - // V = kS + kV * v + kA * a - // 12 = 0.2 + 0.00463 * v - // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV =new double[] { 0, 0, 0, 0 };//: - //new double[] {2.7073, 2.6208, 2.7026, 2.7639};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; - public static final double[] turnkA =new double[] { 0, 0, 0, 0 };//: - //new double[]{0.18069, 0.06593, 0.17439, 0.2571};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; - - // kP is an average of the forward and backward kP values - // Forward: 1.72, 1.71, 1.92, 1.94 - // Backward: 1.92, 1.92, 2.11, 1.89 - // Order of modules: (FL, FR, BL, BR) - public static final double[] drivekP = {1, 1, 1, 1};// CONFIG.isHammerHead() ? new double[] { 0, 0, 0, 0 } - //: new double[] {0,0,0,0};//trust guys //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, - // 1.915/100}; - public static final double[] drivekI = { 0, 0, 0, 0};//CONFIG.isHammerHead()? new double[] {0,0,0,0} : - //new double[] { 0, 0, 0, 0 }; - public static final double[] drivekD = CONFIG.isHammerHead()? new double[] { 0, 0, 0, 0 }: - new double[] { 0,0,0,0 }; - public static final boolean[] driveInversion = (CONFIG.isHammerHead() - ? new boolean[] { true, false, true, false } - : new boolean[] { false, true, false, true }); - public static final boolean[] turnInversion = { true, true, true, true }; - // kS - // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kForwardVolts = CONFIG.isHammerHead() ? new double[] { 0.2,0.2,0.2,0.2 }: - new double[] {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; - public static final double[] kBackwardVolts = kForwardVolts; - - //kV - // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = CONFIG.isHammerHead() ? new double[] { 0,0,0,0 }: - new double[] { 2.9875, 2.9875, 2.7323, 2.9264 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s - public static final double[] kBackwardVels = kForwardVels; - - //kA - // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kForwardAccels = { 0, 0, 0, 0 };//{0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 - public static final double[] kBackwardAccels = kForwardAccels; - public static final double autoMaxSpeedMps = 4;//0.6 * 4.4; // Meters / second public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 public static final double autoMaxAmps = 40.0; // The maximum acceleration the robot can achieve is equal to the coefficient of // static friction times the gravitational acceleration // a = mu * 9.8 m/s^2 - public static final double autoCentripetalAccel = mu * g * 2; public static final boolean isGyroReversed = true; @@ -182,20 +154,6 @@ public static final class Drivetrainc { kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, driveInversion, reversed, driveModifier, turnInversion); - public static final int driveFrontLeftPort = CONFIG.isHammerHead() ? 1 : 1; - public static final int driveFrontRightPort = CONFIG.isHammerHead() ? 2 : 2; - public static final int driveBackLeftPort = CONFIG.isHammerHead() ? 3 : 3; - public static final int driveBackRightPort = CONFIG.isHammerHead() ? 4 : 4; - - public static final int turnFrontLeftPort = CONFIG.isHammerHead() ? 11 : 11; - public static final int turnFrontRightPort = CONFIG.isHammerHead() ? 12 : 12; - public static final int turnBackLeftPort = CONFIG.isHammerHead() ? 13 : 13; - public static final int turnBackRightPort = CONFIG.isHammerHead() ? 14 : 14; - - public static final int canCoderPortFL = CONFIG.isHammerHead() ? 0 : 1; - public static final int canCoderPortFR = CONFIG.isHammerHead() ? 1 : 2; - public static final int canCoderPortBL = CONFIG.isHammerHead() ? 3 : 3; - public static final int canCoderPortBR = CONFIG.isHammerHead() ? 2 : 0; public static double kNormalDriveSpeed = 1; // Percent Multiplier public static double kNormalDriveRotation = 0.5; // Percent Multiplier From eb44b7ff58464320e99c54f489fe6e630f3d30cd Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Wed, 4 Mar 2026 00:26:10 -0800 Subject: [PATCH 02/14] Drivetrain.java clean up --- .../subsystems/Drivetrain.java | 157 +++--------------- 1 file changed, 21 insertions(+), 136 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 45e392e..57cc86f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; @@ -118,8 +117,7 @@ public class Drivetrain extends SubsystemBase { private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); - private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); - // ^used by PathPlanner for chaining paths + private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); //used by PathPlanner for chaining paths private SwerveDriveKinematics kinematics = null; // private SwerveDriveOdometry odometry = null; private SwerveDrivePoseEstimator poseEstimator = null; @@ -163,9 +161,6 @@ public class Drivetrain extends SubsystemBase { public double extraSpeedMult = 0; private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; - double kP = 0; - double kI = 0; - double kD = 0; public enum Mode { coast, brake, @@ -176,19 +171,7 @@ public enum Mode { public Drivetrain(Limelight ll) { this.ll = ll; AutoBuilder(); - //SmartDashboard.putNumber("Goal Velocity", 0); - //SmartDashboard.putNumber("kP", 0); - //SmartDashboard.putNumber("kI", 0); - //SmartDashboard.putNumber("kD", 0); - - // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); - // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); - // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", - // lastSetTheta); - - // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); - // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); - //SmartDashboard.putNumber("GoalPos", 0); + // Calibrate Gyro { @@ -240,7 +223,6 @@ public Drivetrain(Limelight ll) { driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); - //SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), @@ -266,13 +248,6 @@ public Drivetrain(Limelight ll) { }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } - SmartDashboard.putData("Module FL",moduleFL); - SmartDashboard.putData("Module FR",moduleFR); - SmartDashboard.putData("Module BL",moduleBL); - SmartDashboard.putData("Module BR",moduleBR); - - SmartDashboard.putNumber("bigoal", 0); - SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig.openLoopRampRate(secsPer12Volts); driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); @@ -301,10 +276,6 @@ public Drivetrain(Limelight ll) { coder.getVelocity().setUpdateFrequency(500); } - //SmartDashboard.putData("Field", field); - //SmartDashboard.putData("Odometry Field", odometryField); - //martDashboard.putData("Pose with Limelight Field", poseWithLimelightField); - accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); @@ -330,12 +301,6 @@ public Drivetrain(Limelight ll) { // Setup autopath builder //configurePPLAutoBuilder(); - // SmartDashboard.putNumber("chassis speeds x", 0); - // SmartDashboard.putNumber("chassis speeds y", 0); - - // SmartDashboard.putNumber("chassis speeds theta", 0); - SmartDashboard.putData(this); // For seeing drivetrain data in SmartDashboard - } @@ -417,18 +382,8 @@ public void periodic() { detectCollision(); //This does nothing PathPlannerLogging.logCurrentPose(getPose()); - //maybe add the field with the position of the robot with only limelight and the field with the position of the robot with only odometry? - //We can compare the two fields to see if odometry is causing the pose to be inaccurate when it hits the reef. - // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); - // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); - // double goal = SmartDashboard.getNumber("GoalPos", 0); - // PIDController pid = new PIDController(kP, kI, kD); - // kP = SmartDashboard.getNumber("kP", 0); - // kI = SmartDashboard.getNumber("kI", 0); - // kD = SmartDashboard.getNumber("kD", 0); //pid.setIZone(20); - //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); // SparkMaxConfig config = new SparkMaxConfig(); //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); @@ -437,11 +392,6 @@ public void periodic() { // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); // //moduleFL.move(0.0000001, 180); - //moduleFL.move(0.01, 180); - // moduleFR.move(0.000000001, 0); - // moduleBR.move(0.0000001, 0); - // moduleFL.move(0.000001, 0); - // moduleBL.move(0.000001, 0); // turnPidControllers[0].setReference(goal // , ControlType.kPosition, ClosedLoopSlot.kSlot0); @@ -463,18 +413,10 @@ public void periodic() { // i++; // } // lobotomized to prevent ucontrollabe swerve behavior - // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); // moduleFL.periodic(); // moduleFR.periodic(); // moduleBL.periodic(); // moduleBR.periodic(); - double goal = SmartDashboard.getNumber("bigoal", 0); - for (SwerveModule module : modules) { - // module.turnPeriodic(); - // module.turnPeriodic(); - module.move(0.00000000001, goal); - module.periodic(); - } // field.setRobotPose(odometry.getPoseMeters()); @@ -488,13 +430,6 @@ public void periodic() { // updateMT2PoseEstimator(); - // double currSetX = - // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); - // double currSetY = - // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); - // double currSetTheta = SmartDashboard - // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); - // if (lastSetX != currSetX || lastSetY != currSetY // || lastSetTheta != currSetTheta) { // setPose(new Pose2d(currSetX, currSetY, @@ -506,66 +441,37 @@ public void periodic() { // Rotation2d.fromDegrees(getHeading()))); - // SmartDashboard.putNumber("X position with limelight", getPoseWithLimelight().getX()); - // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY()); - SmartDashboard.putNumber("X position with gyro", getPose().getX()); - SmartDashboard.putNumber("Y position with gyro", getPose().getY()); - SmartDashboard.putData(CONFIG); - - //For finding acceleration of drivetrain for collision detector - SmartDashboard.putNumber("Accel X", accelX); - SmartDashboard.putNumber("Accel Y", accelY); - SmartDashboard.putNumber("2D Acceleration ", accelXY); - - // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); - // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); - // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); - // SmartDashboard.putNumber("Robot Heading", getHeading()); - // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); - // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); - // SmartDashboard.putBoolean("Field Oriented", fieldOriented); - // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); - // SmartDashboard.putNumber("Compass Offset", compassOffset); - // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); - SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); - SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); - SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); - SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); } @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); - for (SwerveModule module : modules) + for (SwerveModule module : modules){ SendableRegistry.addChild(this, module); - - builder.addBooleanProperty("Magnetic Field Disturbance", - gyro::isMagneticDisturbance, null); + } + String moduleNames[] = {"FL", "FR", "BL", "BR"}; + for (int i = 0; i < 4; i++) { + final int j = i; //make java happy + builder.addDoubleProperty(moduleNames[j] + "Turn Encoder (Deg)", () -> modules[j].getModuleAngle(), null); + } + SendableRegistry.addChild(this, CONFIG); + builder.addBooleanProperty("Magnetic Field Disturbance", gyro::isMagneticDisturbance, null); builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); - builder.addBooleanProperty("Field Oriented", () -> fieldOriented, - fieldOriented -> this.fieldOriented = fieldOriented); - builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), - null); - builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(), - null); - builder.addDoubleProperty("Pose Estimator Theta", - () -> - getPose().getRotation().getDegrees(), null); + builder.addBooleanProperty("Field Oriented", () -> fieldOriented, fieldOriented -> this.fieldOriented = fieldOriented); + builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), null); + builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(),null); + builder.addDoubleProperty("Pose Estimator Theta", () -> getPose().getRotation().getDegrees(), null); + builder.addDoubleProperty("X position with gyro", () -> getPose().getX(), null); + builder.addDoubleProperty("Y position with gyro", () -> getPose().getY(), null); builder.addDoubleProperty("Robot Heading", () -> getHeading(), null); builder.addDoubleProperty("Raw Gyro Angle", gyro::getAngle, null); builder.addDoubleProperty("Pitch", gyro::getPitch, null); builder.addDoubleProperty("Roll", gyro::getRoll, null); - builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> - this.fieldOffset = fieldOffset); - builder.addDoubleProperty("FL Turn Encoder (Deg)", - () -> moduleFL.getModuleAngle(), null); - builder.addDoubleProperty("FR Turn Encoder (Deg)", - () -> moduleFR.getModuleAngle(), null); - builder.addDoubleProperty("BL Turn Encoder (Deg)", - () -> moduleBL.getModuleAngle(), null); - builder.addDoubleProperty("BR Turn Encoder (Deg)", - () -> moduleBR.getModuleAngle(), null); + builder.addDoubleProperty("Accel X", () -> accelX, null); + builder.addDoubleProperty("Accel Y", () -> accelY, null); + builder.addDoubleProperty("2D Acceleration ", () -> accelXY, null); + builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> this.fieldOffset = fieldOffset); } @@ -602,9 +508,7 @@ public void drive(SwerveModuleState[] moduleStates) { double max = maxSpeed; SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, max); for (int i = 0; i < 4; i++) { - // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); moduleStates[i].optimize(Rotation2d.fromDegrees(modules[i].getModuleAngle())); - // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); } } @@ -788,15 +692,6 @@ public boolean detectCollision(){ //We can implement this method into updatePose return accelXY > COLLISION_ACCELERATION_THRESHOLD; // return true if collision detected } - /** - * @deprecated Use {@link #resetFieldOrientation()} instead - */ - @Deprecated - public void resetHeading() { - gyro.reset(); - - } - public double getPitch() { return gyro.getPitch(); } @@ -895,8 +790,6 @@ public void coast() { module.coast(); } - - // #region SysId Code // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. @@ -1111,11 +1004,6 @@ private void sysIdSetup() { // sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); // sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); - // SmartDashboard.putData("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); - // SmartDashboard.putData("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); - - // SmartDashboard.putData("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); - // SmartDashboard.putData("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); //sysIdTab.add(this); for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors @@ -1129,9 +1017,6 @@ private void sysIdSetup() { m_revs_vel[i] = RotationsPerSecond.mutable(0); } - // SmartDashboard.putNumber("Desired Angle", 0); - - // SmartDashboard.putNumber("kS", 0); } } From d970c9814989e9b558a1ec8b95657fdea6600b44 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Wed, 4 Mar 2026 13:15:56 -0800 Subject: [PATCH 03/14] make Drivetrain use dynamic motor controller type based on config --- .../java/org/carlmontrobotics/Config.java | 2 +- .../java/org/carlmontrobotics/Constants.java | 7 ++- .../subsystems/Drivetrain.java | 45 ++++++++----------- 3 files changed, 25 insertions(+), 29 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index 14c4b57..3d5ec99 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -22,7 +22,7 @@ public abstract class Config implements Sendable { // Add additional config settings by declaring a protected field, and... protected boolean exampleFlagEnabled = false; - protected boolean vortexDrive = true; + protected boolean vortexDrive = isHammerHead() ? true : false; protected boolean hammerHead = true; protected boolean setupSysId = false; protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index afd896e..650cdbc 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -74,6 +74,9 @@ public static final class Drivetrainc { public static final int turnFrontRightPort = 12; public static final int turnBackLeftPort = 13; public static final int turnBackRightPort = 14; + + public static final MotorConfig driveMotorConfig = CONFIG.isVortexDrive() ? MotorConfig.NEO_VORTEX : MotorConfig.NEO; + public static final MotorConfig turnMotorConfig = MotorConfig.NEO; //TODO: set hammerhead can coder ports the same as kitbot public static final int canCoderPortFL = CONFIG.isHammerHead() ? 0 : 1; public static final int canCoderPortFR = CONFIG.isHammerHead() ? 1 : 2; @@ -102,9 +105,9 @@ public static final class Drivetrainc { public static final double[] drivekP = {1, 1, 1, 1}; public static final double[] drivekI = {0, 0, 0, 0}; public static final double[] drivekD = CONFIG.isHammerHead()? new double[] { 0, 0, 0, 0 }: - new double[] { 0,0,0,0 }; + new double[] { 0,0,0,0 }; public static final double[] turnkP = CONFIG.isHammerHead() ? new double[] {50, 50, 50, 50} : - new double[] {0,0,0,0}; + new double[] {0,0,0,0}; public static final double[] turnkI = {0, 0, 0, 0}; public static final double[] turnkD = {0, 0, 0, 0}; public static final double[] turnkS = {1, 1, 1, 1}; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 57cc86f..a036bd6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -5,7 +5,6 @@ import java.util.function.Supplier; //lib199 -import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.SensorFactory; import org.carlmontrobotics.lib199.swerve.SwerveModule; @@ -69,13 +68,11 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -126,8 +123,8 @@ public class Drivetrain extends SubsystemBase { private boolean fieldOriented = true; private double fieldOffset = 0; // FIXME not for permanent use!! - private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; - private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; + private SparkBase[] driveMotors = new SparkBase[] { null, null, null, null }; + private SparkBase[] turnMotors = new SparkBase[] { null, null, null, null }; private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; public final float initPitch; @@ -220,22 +217,22 @@ public Drivetrain(Limelight ll) { moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), - turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + driveMotors[0] = MotorControllerFactory.createSpark(driveFrontLeftPort, driveMotorConfig), + turnMotors[0] = MotorControllerFactory.createSpark(turnFrontLeftPort, turnMotorConfig), turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), - turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + driveMotors[1] = MotorControllerFactory.createSpark(driveFrontRightPort, driveMotorConfig), + turnMotors[1] = MotorControllerFactory.createSpark(turnFrontRightPort, turnMotorConfig), turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, - driveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), - turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + driveMotors[2] = MotorControllerFactory.createSpark(driveBackLeftPort, driveMotorConfig), + turnMotors[2] = MotorControllerFactory.createSpark(turnBackLeftPort, turnMotorConfig), turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, - driveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), - turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + driveMotors[3] = MotorControllerFactory.createSpark(driveBackRightPort, driveMotorConfig), + turnMotors[3] = MotorControllerFactory.createSpark(turnBackRightPort, turnMotorConfig), turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; turnPidControllers[0] = turnMotors[0].getClosedLoopController(); @@ -248,25 +245,24 @@ public Drivetrain(Limelight ll) { }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } - SparkMaxConfig driveConfig = new SparkMaxConfig(); + SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); driveConfig.openLoopRampRate(secsPer12Volts); driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); driveConfig.encoder.uvwAverageDepth(2); driveConfig.encoder.uvwMeasurementPeriod(16); - driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); - for (SparkMax driveMotor : driveMotors) { + for (SparkBase driveMotor : driveMotors) { driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } - SparkMaxConfig turnConfig = new SparkMaxConfig(); + SparkBaseConfig turnConfig = MotorControllerFactory.sparkConfig(turnMotorConfig); turnConfig.encoder.positionConversionFactor(360/turnGearing); turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); turnConfig.encoder.uvwAverageDepth(2); turnConfig.encoder.uvwMeasurementPeriod(16); //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - for (SparkMax turnMotor : turnMotors) { + for (SparkBase turnMotor : turnMotors) { turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } @@ -280,9 +276,6 @@ public Drivetrain(Limelight ll) { accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); - // for(SparkMax driveMotor : driveMotors) - // driveMotor.setSmartCurrentLimit(80); - // Must call this method for SysId to run if (CONFIG.isSysIdTesting()) { sysIdSetup(); @@ -365,13 +358,13 @@ public void setDrivingIdleMode(boolean brake) { else { mode = IdleMode.kCoast; } - for (SparkMax turnMotor : turnMotors) { - SparkMaxConfig config = new SparkMaxConfig(); + for (SparkBase turnMotor : turnMotors) { + SparkBaseConfig config = MotorControllerFactory.createConfig(turnMotorConfig.controllerType); config.idleMode(mode); turnMotor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } - for (SparkMax driveMotor : driveMotors) { - SparkMaxConfig config = new SparkMaxConfig(); + for (SparkBase driveMotor : driveMotors) { + SparkBaseConfig config = MotorControllerFactory.createConfig(driveMotorConfig.controllerType); config.idleMode(mode); driveMotor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } @@ -877,7 +870,7 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { defaultSysIdConfig, new SysIdRoutine.Mechanism( (Voltage volts) -> { - for (SparkMax dm : driveMotors) { + for (SparkBase dm : driveMotors) { dm.setVoltage(volts.in(Volts)); } }, From 2a91bff522f739c4154bb395e58a000cd751f7ce Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Thu, 5 Mar 2026 13:07:57 -0800 Subject: [PATCH 04/14] bye bye sysid --- .../subsystems/Drivetrain.java | 423 ------------------ 1 file changed, 423 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index a036bd6..bc088db 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj2.command.Command; @@ -33,7 +32,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; //math import edu.wpi.first.math.MathUtil; @@ -276,10 +274,6 @@ public Drivetrain(Limelight ll) { accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); - // Must call this method for SysId to run - if (CONFIG.isSysIdTesting()) { - sysIdSetup(); - } } // odometry = new SwerveDriveOdometry(kinematics, @@ -333,19 +327,6 @@ public void simulationPeriodic() { gyroYawSim.set(newAngleDeg); } - // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int - // frontorback) { - // switch(frontorback) { - // case 0: - // return frontOnlyRoutine.quasistatic(direction); - // case 1: - // return backOnlyRoutine.quasistatic(direction); - // case 2: - // return allWheelsRoutine.quasistatic(direction); - // } - // return new PrintCommand("Invalid Command"); - // } - /** * Sets swerveModules IdleMode both turn and drive * @param brake boolean for braking, if false then coast @@ -783,410 +764,6 @@ public void coast() { module.coast(); } - // #region SysId Code - - // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutVoltage[] m_appliedVoltage = new MutVoltage[8]; - - // Mutable holder for unit-safe linear distance values, persisted to avoid - // reallocation. - private final MutDistance[] m_distance = new MutDistance[4]; - // Mutable holder for unit-safe linear velocity values, persisted to avoid - // reallocation. - private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[4]; - // edu.wpi.first.math.util.Units.Rotations beans; - private final MutAngle[] m_revs = new MutAngle[4]; - private final MutAngularVelocity[] m_revs_vel = new MutAngularVelocity[4]; - - private enum SysIdTest { - FRONT_DRIVE, - BACK_DRIVE, - ALL_DRIVE, - // FLBR_TURN, - // FRBL_TURN, - // ALL_TURN - FL_ROT, - FR_ROT, - BL_ROT, - BR_ROT - } - - private SendableChooser sysIdChooser = new SendableChooser<>(); - - // ROUTINES FOR SYSID - // private SysIdRoutine.Config defaultSysIdConfig = new - // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), - // Seconds.of(5)); - private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), - Volts.of(2.891), Seconds.of(10)); - - // DRIVE - private void motorLogShort_drive(SysIdRoutineLog log, int id) { - String name = new String[] { "fl", "fr", "bl", "br" }[id]; - log.motor(name) - .voltage(m_appliedVoltage[id].mut_replace( - driveMotors[id].getBusVoltage() * driveMotors[id].getAppliedOutput(), Volts)) - .linearPosition( - m_distance[id].mut_replace(driveMotors[id].getEncoder().getPosition(), Meters)) - .linearVelocity(m_velocity[id].mut_replace(driveMotors[id].getEncoder().getVelocity(), - MetersPerSecond)); - } - - // Create a new SysId routine for characterizing the drive. - private SysIdRoutine frontOnlyDriveRoutine = new SysIdRoutine( - defaultSysIdConfig, - new SysIdRoutine.Mechanism( - // Tell SysId how to give the driving voltage to the motors. - (Voltage volts) -> { - driveMotors[0].setVoltage(volts.in(Volts)); - driveMotors[1].setVoltage(volts.in(Volts)); - modules[2].coast(); - modules[3].coast(); - }, - log -> {// FRONT - motorLogShort_drive(log, 0);// fl named automatically - motorLogShort_drive(log, 1);// fr - }, - this)); - - private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( - defaultSysIdConfig, - new SysIdRoutine.Mechanism( - (Voltage volts) -> { - modules[0].coast(); - modules[1].coast(); - modules[2].brake(); - modules[3].brake(); - driveMotors[2].setVoltage(volts.in(Volts)); - driveMotors[3].setVoltage(volts.in(Volts)); - }, - log -> {// BACK - motorLogShort_drive(log, 2);// bl - motorLogShort_drive(log, 3);// br - }, - this)); - - private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( - defaultSysIdConfig, - new SysIdRoutine.Mechanism( - (Voltage volts) -> { - for (SparkBase dm : driveMotors) { - dm.setVoltage(volts.in(Volts)); - } - }, - log -> { - motorLogShort_drive(log, 0);// fl named automatically - motorLogShort_drive(log, 1);// fr - motorLogShort_drive(log, 2);// bl - motorLogShort_drive(log, 3);// br - }, - this)); - - private SysIdRoutine sysidroutshort_turn(int id, String logname) { - return new SysIdRoutine( - defaultSysIdConfig, - // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), - // Seconds.of(3)), - new SysIdRoutine.Mechanism( - (Voltage volts) -> turnMotors[id].setVoltage(volts.in(Volts)), - log -> log.motor(logname + "_turn") - .voltage(m_appliedVoltage[id + 4].mut_replace( - // ^because drivemotors take up the first 4 slots of the unit holders - turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) - .angularPosition( - m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) - .angularVelocity(m_revs_vel[id].mut_replace( - turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), - this)); - } - - // as always, fl/fr/bl/br - private SysIdRoutine[] rotateRoutine = new SysIdRoutine[] { - sysidroutshort_turn(0, "fl"), // woaw, readable code??? - sysidroutshort_turn(1, "fr"), - sysidroutshort_turn(2, "bl"), - sysidroutshort_turn(3, "br") - }; - - //TODO: migrate to elastic - //private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Drivetrain SysID"); - - // void sysidtabshorthand(String name, SysIdRoutine.Direction dir, int width, - // int height){ - // sysIdTab.add(name, dir).withSize(width, height); - // } - void sysidtabshorthand_qsi(String name, SysIdRoutine.Direction dir) { - //TODO: migrate to elastic - //sysIdTab.add(name, sysIdQuasistatic(dir)).withSize(2, 1); - } - - // void sysidtabshorthand_dyn(String name, SysIdRoutine.Direction dir) { - // sysIdTab.add(name, sysIdDynamic(dir)).withSize(2, 1); - // } - - private void sysIdSetup() { - // SysId Setup - { - Supplier stopNwait = () -> new SequentialCommandGroup( - new InstantCommand(this::stop), new WaitCommand(2)); - - /* - * Alex's old sysId tests - * sysIdTab.add("All sysid tests", new SequentialCommandGroup( - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,2), - * (Command)stopNwait.get()), - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,2), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,2), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,2), - * (Command)stopNwait.get()) - * )); - * sysIdTab.add("All sysid tests - FRONT wheels", new SequentialCommandGroup( - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,0), - * (Command)stopNwait.get()), - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,0), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,0), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,0), - * (Command)stopNwait.get()) - * )); - * sysIdTab.add("All sysid tests - BACK wheels", new SequentialCommandGroup( - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,1), - * (Command)stopNwait.get()), - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,1), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,1), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,1), - * (Command)stopNwait.get()) - * )); - */ - - // sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); - // sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); - // sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); - // sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); - - //TODO: migrate to elastic - //sysIdTab - //.add(sysIdChooser) - //.withSize(2, 1); - - sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); - sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); - sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); - // sysIdChooser.addOption("fl-br Turn", SysIdTest.FLBR_TURN); - // sysIdChooser.addOption("fr-bl Turn", SysIdTest.FRBL_TURN); - // sysIdChooser.addOption("All Turn", SysIdTest.ALL_TURN); - sysIdChooser.addOption("FL Rotate", SysIdTest.FL_ROT); - sysIdChooser.addOption("FR Rotate", SysIdTest.FR_ROT); - sysIdChooser.addOption("BL Rotate", SysIdTest.BL_ROT); - sysIdChooser.addOption("BR Rotate", SysIdTest.BR_ROT); - - //TODO: migrate to elastic - //sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? - //.withSize(2, 1); - - // sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); - // sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); - //sysIdTab.add(this); - - for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors - m_appliedVoltage[i] = Volt.mutable(0); - } - for (int i = 0; i < 4; i++) { - m_distance[i] = Meter.mutable(0); - m_velocity[i] = MetersPerSecond.mutable(0); - - m_revs[i] = Rotation.mutable(0); - m_revs_vel[i] = RotationsPerSecond.mutable(0); - } - - } - } - - // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int - // frontorback) { - // switch(frontorback) { - // case 0: - // return frontOnlyRoutine.quasistatic(direction); - // case 1: - // return backOnlyRoutine.quasistatic(direction); - // case 2: - // return allWheelsRoutine.quasistatic(direction); - // } - // return new PrintCommand("Invalid Command"); - // } - - private SysIdTest selector() { - //SysIdTest test = sysIdChooser.getSelected(); - SysIdTest test = SysIdTest.FRONT_DRIVE; - System.out.println("Test Selected: " + test); - return test; - } - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return new SelectCommand<>( - Map.ofEntries( - // DRIVE - Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running front only quasistatic forward") - : new PrintCommand("Running front only quasistatic backward"), - frontOnlyDriveRoutine.quasistatic(direction))), - Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running back only quasistatic forward") - : new PrintCommand("Running back only quasistatic backward"), - backOnlyDriveRoutine.quasistatic(direction))), - Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running all drive quasistatic forward") - : new PrintCommand("Running all drive quasistatic backward"), - allWheelsDriveRoutine.quasistatic(direction))), - // ROTATE - Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running FL rotate quasistatic forward") - : new PrintCommand("Running FL rotate quasistatic backward"), - rotateRoutine[0].quasistatic(direction))), - Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running FR rotate quasistatic forward") - : new PrintCommand("Running FR rotate quasistatic backward"), - rotateRoutine[1].quasistatic(direction))), - Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running BL rotate quasistatic forward") - : new PrintCommand("Running BL rotate quasistatic backward"), - rotateRoutine[2].quasistatic(direction))), - Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( - direction == SysIdRoutine.Direction.kForward - ? new PrintCommand("Running BR rotate quasistatic forward") - : new PrintCommand("Running BR rotate quasistatic backward"), - rotateRoutine[3].quasistatic(direction))) - - // //TURN - // Map.entry(SysIdTest.FLBR_TURN, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward ? - // new PrintCommand("Running fL-bR turn quasistatic forward") : - // new PrintCommand("Running fL-bR turn quasistatic backward"), - // flbrTurn.quasistatic(direction) - // )), - // Map.entry(SysIdTest.FRBL_TURN, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward ? - // new PrintCommand("Running fR-bL turn quasistatic forward") : - // new PrintCommand("Running fR-bL turn quasistatic backward"), - // frblTurn.quasistatic(direction) - // )), - // Map.entry(SysIdTest.ALL_TURN, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward ? - // new PrintCommand("Running all turn quasistatic forward") : - // new PrintCommand("Running all turn quasistatic backward"), - // allWheelsTurn.quasistatic(direction) - // )) - ), - this::selector); - } - - // public Command sysIdDynamic(SysIdRoutine.Direction direction, int - // frontorback) { - // switch(frontorback) { - // case 0: - // return frontOnlyDrive.dynamic(direction); - // case 1: - // return backOnlyDrive.dynamic(direction); - // case 2: - // return allWheelsDrive.dynamic(direction); - // } - // return new PrintCommand("Invalid Command"); - // } - private Command allTheSYSID(SysIdRoutine.Direction direction) { - return new SequentialCommandGroup( - frontOnlyDriveRoutine.dynamic(direction), - backOnlyDriveRoutine.dynamic(direction), - allWheelsDriveRoutine.dynamic(direction), - rotateRoutine[0].dynamic(direction), - rotateRoutine[1].dynamic(direction), - rotateRoutine[2].dynamic(direction), - rotateRoutine[3].dynamic(direction), - - frontOnlyDriveRoutine.quasistatic(direction), - backOnlyDriveRoutine.quasistatic(direction), - allWheelsDriveRoutine.quasistatic(direction), - rotateRoutine[0].quasistatic(direction), - rotateRoutine[1].quasistatic(direction), - rotateRoutine[2].quasistatic(direction), - rotateRoutine[3].quasistatic(direction)); - } - - /** - * Makes sysId to run for both directions - * @return Command to run sysId - */ - public Command allTheSYSID() { - return new SequentialCommandGroup( - allTheSYSID(SysIdRoutine.Direction.kForward), - allTheSYSID(SysIdRoutine.Direction.kReverse)); - } - /** - * Makes sysId to find feedforward and pid values for drivetrain - * @param direction SysIdRoutine.Direction.kForward or kReverse - * @return Command to run sysID - */ - // public Command sysIdDynamic(SysIdRoutine.Direction direction) { - // return new SelectCommand<>( - // Map.ofEntries( - // // DRIVE - // Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running front only dynamic forward") - // : new PrintCommand("Running front only dynamic backward"), - // frontOnlyDriveRoutine.dynamic(direction))), - // Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running back only dynamic forward") - // : new PrintCommand("Running back only dynamic backward"), - // backOnlyDriveRoutine.dynamic(direction))), - // Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running all wheels dynamic forward") - // : new PrintCommand("Running all wheels dynamic backward"), - // allWheelsDriveRoutine.dynamic(direction))), - // // ROTATE - // Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running FL rotate dynamic forward") - // : new PrintCommand("Running FL rotate dynamic backward"), - // rotateRoutine[0].dynamic(direction))), - // Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running FR rotate dynamic forward") - // : new PrintCommand("Running FR rotate dynamic backward"), - // rotateRoutine[1].dynamic(direction))), - // Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running BL rotate dynamic forward") - // : new PrintCommand("Running BL rotate dynamic backward"), - // rotateRoutine[2].dynamic(direction))), - // Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( - // direction == SysIdRoutine.Direction.kForward - // ? new PrintCommand("Running BR rotate dynamic forward") - // : new PrintCommand("Running BR rotate dynamic backward"), - // rotateRoutine[3].dynamic(direction)))), - // this::selector); - // } - - // #endregion - /** * Sets all SwerveModules to point in a certain angle * @param angle in degrees From f43fe0066c3d7e5a9588630f46254c9854c42884 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 6 Mar 2026 10:03:40 -0800 Subject: [PATCH 05/14] clean up dt --- .../java/org/carlmontrobotics/Constants.java | 6 + .../subsystems/Drivetrain.java | 127 ++---------------- 2 files changed, 16 insertions(+), 117 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 650cdbc..1741e77 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -183,6 +183,12 @@ public static final class Drivetrainc { public static final double driveIzone = .1; public static final double COLLISION_ACCELERATION_THRESHOLD = 2; //The minimum acceleration that will trigger a collision detection, in m/s^2 + public static final double ppkPDrive = 5; + public static final double ppkIDrive = 0; + public static final double ppkDDrive = 0; + public static final double ppkPTurn = 3; + public static final double ppkITurn = 0; + public static final double ppkDTurn = 0; public static final class Autoc { public static final RobotConfig robotConfig = new RobotConfig( // Mass mass, kg diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index bc088db..0b3b9c5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -38,19 +38,14 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.controller.PIDController; //vendordeps -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.CANcoder; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; @@ -63,30 +58,18 @@ import com.pathplanner.lib.config.RobotConfig; //rev -import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; //units -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; import static edu.wpi.first.units.Units.Volts; @@ -100,14 +83,9 @@ //Constants import org.carlmontrobotics.Constants; -import org.carlmontrobotics.Constants.Drivetrainc; -import org.carlmontrobotics.Constants.Drivetrainc.Autoc; -import org.carlmontrobotics.Robot; -import org.carlmontrobotics.subsystems.Limelight; import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; import static org.carlmontrobotics.Constants.Drivetrainc.*; -import static org.carlmontrobotics.Constants.LimeLightc.*; public class Drivetrain extends SubsystemBase { @@ -118,13 +96,13 @@ public class Drivetrain extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator = null; private SwerveModule modules[]; + private String moduleNames[] = {"FL", "FR", "BL", "BR"}; private boolean fieldOriented = true; private double fieldOffset = 0; // FIXME not for permanent use!! private SparkBase[] driveMotors = new SparkBase[] { null, null, null, null }; private SparkBase[] turnMotors = new SparkBase[] { null, null, null, null }; private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; - private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; public final float initPitch; public final float initRoll; @@ -138,14 +116,6 @@ public class Drivetrain extends SubsystemBase { private final Field2d odometryField = new Field2d(); private final Field2d poseWithLimelightField = new Field2d(); - public double ppKpDrive = 5.0; - public double ppKiDrive = 0; - public double ppKdDrive = 0; - - public double ppKpTurn = 3; - public double ppKiTurn = 0; - public double ppKdTurn = 0; - double accelX; double accelY; double accelXY; @@ -218,6 +188,7 @@ public Drivetrain(Limelight ll) { driveMotors[0] = MotorControllerFactory.createSpark(driveFrontLeftPort, driveMotorConfig), turnMotors[0] = MotorControllerFactory.createSpark(turnFrontLeftPort, turnMotorConfig), turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, driveMotors[1] = MotorControllerFactory.createSpark(driveFrontRightPort, driveMotorConfig), turnMotors[1] = MotorControllerFactory.createSpark(turnFrontRightPort, turnMotorConfig), @@ -232,14 +203,12 @@ public Drivetrain(Limelight ll) { driveMotors[3] = MotorControllerFactory.createSpark(driveBackRightPort, driveMotorConfig), turnMotors[3] = MotorControllerFactory.createSpark(turnBackRightPort, turnMotorConfig), turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; - turnPidControllers[0] = turnMotors[0].getClosedLoopController(); - turnPidControllers[1] = turnMotors[1].getClosedLoopController(); - turnPidControllers[2] = turnMotors[2].getClosedLoopController(); - turnPidControllers[3] = turnMotors[3].getClosedLoopController(); + if (RobotBase.isSimulation()) { moduleSims = new SwerveModuleSim[] { - moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() //FIXME this is with values based off of hammerhead }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } @@ -259,7 +228,6 @@ public Drivetrain(Limelight ll) { turnConfig.encoder.uvwAverageDepth(2); turnConfig.encoder.uvwMeasurementPeriod(16); - //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); for (SparkBase turnMotor : turnMotors) { turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } @@ -276,10 +244,6 @@ public Drivetrain(Limelight ll) { } - // odometry = new SwerveDriveOdometry(kinematics, - // Rotation2d.fromDegrees(getHeading()), getModulePositions(), - // new Pose2d()); - poseEstimator = new SwerveDrivePoseEstimator( getKinematics(), Rotation2d.fromDegrees(getHeading()), @@ -355,76 +319,15 @@ public void setDrivingIdleMode(boolean brake) { public void periodic() { detectCollision(); //This does nothing PathPlannerLogging.logCurrentPose(getPose()); - - - //pid.setIZone(20); - // SparkMaxConfig config = new SparkMaxConfig(); - - //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); - // System.out.println(kP); - // config.closedLoop.pid(kP ,kI,kD); - // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); - // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - // //moduleFL.move(0.0000001, 180); - // turnPidControllers[0].setReference(goal - - // , ControlType.kPosition, ClosedLoopSlot.kSlot0); - - - // 167 -> -200 - // 138 -> 360 - // for (CANcoder coder : turnEncoders) { - // SignalLogger.writeDouble("Regular position " + coder.toString(), - // coder.getPosition().getValue().baseUnitMagnitude()); - // SignalLogger.writeDouble("Velocity " + coder.toString(), - // coder.getVelocity().getValue().baseUnitMagnitude()); - // SignalLogger.writeDouble("Absolute position " + coder.toString(), - // coder.getAbsolutePosition().getValue().baseUnitMagnitude()); - // } - // String out=""; int i=0; - // for (CANcoder coder : turnEncoders) { - // out+=String.format("[i] Abs Pos: %.3f Goal Pos: %.3f ", coder.getAbsolutePosition().getValue().baseUnitMagnitude(),0); - // i++; - // } - // lobotomized to prevent ucontrollabe swerve behavior - // moduleFL.periodic(); - // moduleFR.periodic(); - // moduleBL.periodic(); - // moduleBR.periodic(); - - // field.setRobotPose(odometry.getPoseMeters()); - - - - // odometry.update(gyro.getRotation2d(), getModulePositions()); - - // poseEstimator.update(gyro.getRotation2d(), getModulePositions()); - - //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); - - // updateMT2PoseEstimator(); - - // if (lastSetX != currSetX || lastSetY != currSetY - // || lastSetTheta != currSetTheta) { - // setPose(new Pose2d(currSetX, currSetY, - // Rotation2d.fromDegrees(currSetTheta))); - // } - - // setPose(new Pose2d(getPose().getTranslation().getX(), - // getPose().getTranslation().getY(), - // Rotation2d.fromDegrees(getHeading()))); - - } @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); - + builder.setSafeState(this::stop); for (SwerveModule module : modules){ SendableRegistry.addChild(this, module); } - String moduleNames[] = {"FL", "FR", "BL", "BR"}; for (int i = 0; i < 4; i++) { final int j = i; //make java happy builder.addDoubleProperty(moduleNames[j] + "Turn Encoder (Deg)", () -> modules[j].getModuleAngle(), null); @@ -503,11 +406,9 @@ public void AutoBuilder() { (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards //PathFollowingController controller, new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(4 - , ppKiDrive, ppKdDrive), // Translation PID constants - new PIDConstants(1, ppKiTurn, ppKdTurn) + new PIDConstants(ppkPDrive, ppkIDrive, ppkDDrive), // Translation PID constants + new PIDConstants(ppkPTurn, ppkITurn, ppkDTurn) ), - //RobotConfig robotConfig, config, // The robot configuration //BooleanSupplier shouldFlipPath, () -> { @@ -739,26 +640,18 @@ public void setMode(Mode mode) { } } /** - * @deprecated Use {@link #setMode(Mode)} instead * Changes between IdleModes */ - @Deprecated public void toggleMode() { for (SwerveModule module : modules) module.toggleMode(); } - /** - * @deprecated Use {@link #setMode(Mode)} instead - */ - @Deprecated + public void brake() { for (SwerveModule module : modules) module.brake(); } - /** - * @deprecated Use {@link #setMode(Mode)} instead - */ - @Deprecated + public void coast() { for (SwerveModule module : modules) module.coast(); From f964853703e51b557b60a05bdf71683360168b37 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 6 Mar 2026 10:03:51 -0800 Subject: [PATCH 06/14] simplify dt module creation --- .../java/org/carlmontrobotics/Constants.java | 5 ++ .../subsystems/Drivetrain.java | 66 +++++++------------ 2 files changed, 29 insertions(+), 42 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 1741e77..0472157 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -65,6 +65,7 @@ public static final class Manipulator { //#region Drivetrain public static final class Drivetrainc { //general drivetrain constants + public static final int driveFrontLeftPort = 1; public static final int driveFrontRightPort = 2; public static final int driveBackLeftPort = 3; @@ -83,6 +84,10 @@ public static final class Drivetrainc { public static final int canCoderPortBL = CONFIG.isHammerHead() ? 3 : 3; public static final int canCoderPortBR = CONFIG.isHammerHead() ? 2 : 0; + public static final int drivePorts[] = { driveFrontLeftPort, driveFrontRightPort, driveBackLeftPort, driveBackRightPort }; + public static final int turnPorts[] = { turnFrontLeftPort, turnFrontRightPort, turnBackLeftPort, turnBackRightPort }; + public static final int canCoderPorts[] = { canCoderPortFL, canCoderPortFR, canCoderPortBL, canCoderPortBR }; + // swerve config constants public static final double wheelBase = CONFIG.isHammerHead() ? Units.inchesToMeters(16.750003) : Units.inchesToMeters(16.750003); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 0b3b9c5..062cc78 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -137,6 +137,7 @@ public Drivetrain(Limelight ll) { this.ll = ll; AutoBuilder(); + modules = new SwerveModule[4]; // Calibrate Gyro { @@ -183,28 +184,30 @@ public Drivetrain(Limelight ll) { // Supplier pitchSupplier = () -> gyro.getPitch(); // Supplier rollSupplier = () -> gyro.getRoll(); + SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); + driveConfig.openLoopRampRate(secsPer12Volts) + .encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing) + .velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60) + .uvwAverageDepth(2) + .uvwMeasurementPeriod(16); - moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = MotorControllerFactory.createSpark(driveFrontLeftPort, driveMotorConfig), - turnMotors[0] = MotorControllerFactory.createSpark(turnFrontLeftPort, turnMotorConfig), - turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); - - moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = MotorControllerFactory.createSpark(driveFrontRightPort, driveMotorConfig), - turnMotors[1] = MotorControllerFactory.createSpark(turnFrontRightPort, turnMotorConfig), - turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); - - moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, - driveMotors[2] = MotorControllerFactory.createSpark(driveBackLeftPort, driveMotorConfig), - turnMotors[2] = MotorControllerFactory.createSpark(turnBackLeftPort, turnMotorConfig), - turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); - - moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, - driveMotors[3] = MotorControllerFactory.createSpark(driveBackRightPort, driveMotorConfig), - turnMotors[3] = MotorControllerFactory.createSpark(turnBackRightPort, turnMotorConfig), - turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); - - modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkBaseConfig turnConfig = MotorControllerFactory.sparkConfig(turnMotorConfig); + turnConfig.encoder.positionConversionFactor(360/turnGearing) + .velocityConversionFactor(360/turnGearing/60) + .uvwAverageDepth(2) + .uvwMeasurementPeriod(16); + + for (int i = 0; i < modules.length; i++) { + modules[i] = new SwerveModule(swerveConfig, SwerveModule.ModuleType.values()[i], + driveMotors[i] = MotorControllerFactory.createSpark(drivePorts[i], driveMotorConfig, driveConfig), + turnMotors[i] = MotorControllerFactory.createSpark(turnPorts[i], turnMotorConfig, turnConfig), + turnEncoders[i] = SensorFactory.createCANCoder(canCoderPorts[i]), i, pitchSupplier, rollSupplier); + } + + moduleFL = modules[0]; + moduleFR = modules[1]; + moduleBL = modules[2]; + moduleBR = modules[3]; if (RobotBase.isSimulation()) { moduleSims = new SwerveModuleSim[] { @@ -212,25 +215,6 @@ public Drivetrain(Limelight ll) { }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } - SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); - driveConfig.openLoopRampRate(secsPer12Volts); - driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); - driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); - driveConfig.encoder.uvwAverageDepth(2); - driveConfig.encoder.uvwMeasurementPeriod(16); - - for (SparkBase driveMotor : driveMotors) { - driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - } - SparkBaseConfig turnConfig = MotorControllerFactory.sparkConfig(turnMotorConfig); - turnConfig.encoder.positionConversionFactor(360/turnGearing); - turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); - turnConfig.encoder.uvwAverageDepth(2); - turnConfig.encoder.uvwMeasurementPeriod(16); - - for (SparkBase turnMotor : turnMotors) { - turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - } for (CANcoder coder : turnEncoders) { coder.getAbsolutePosition().setUpdateFrequency(500); @@ -254,8 +238,6 @@ public Drivetrain(Limelight ll) { //configurePPLAutoBuilder(); } - - public boolean isAtAngle(double desiredAngleDeg, double toleranceDeg){ for (SwerveModule module : modules) { if (!(Math.abs(MathUtil.inputModulus(module.getModuleAngle() - desiredAngleDeg, -90, 90)) < toleranceDeg)) From 23e76a9b3b1979d79bd7241c63ef7874f446aacd Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 6 Mar 2026 10:08:07 -0800 Subject: [PATCH 07/14] organize imports --- .../subsystems/Drivetrain.java | 94 ++++++------------- 1 file changed, 28 insertions(+), 66 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 062cc78..c07efae 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -1,39 +1,34 @@ package org.carlmontrobotics.subsystems; +import static org.carlmontrobotics.Config.CONFIG; +import static org.carlmontrobotics.Constants.Drivetrainc.*; + import java.util.Arrays; -import java.util.Map; import java.util.function.Supplier; -//lib199 +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; +import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.SensorFactory; import org.carlmontrobotics.lib199.swerve.SwerveModule; import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; -import static org.carlmontrobotics.Config.CONFIG; +import com.ctre.phoenix6.hardware.CANcoder; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.hal.SimDouble; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; - -//wpilib -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.SimDeviceSim; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SelectCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -//math import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -44,48 +39,15 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; - -//vendordeps -import com.ctre.phoenix6.hardware.CANcoder; -import com.studica.frc.AHRS; -import com.studica.frc.AHRS.NavXComType; - -//pathplanner -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; - -//rev -import com.revrobotics.spark.SparkBase; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -//units -import edu.wpi.first.units.measure.MutAngle; -import edu.wpi.first.units.measure.MutAngularVelocity; -import edu.wpi.first.units.measure.MutDistance; -import edu.wpi.first.units.measure.MutLinearVelocity; -import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.measure.Voltage; - -import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volt; -import static edu.wpi.first.units.Units.Meter; -import static edu.wpi.first.units.Units.Meters; - -//Constants -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; -import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; -import static org.carlmontrobotics.Constants.Drivetrainc.*; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Drivetrain extends SubsystemBase { From 8099fc7802ce7ce4943543d7d4fd4a2951fe2545 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 6 Mar 2026 12:38:47 -0800 Subject: [PATCH 08/14] more dt clean up --- .../subsystems/Drivetrain.java | 41 +++++++------------ 1 file changed, 14 insertions(+), 27 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index c07efae..6317625 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -9,6 +9,7 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; +import org.carlmontrobotics.lib199.Limelight; import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.SensorFactory; import org.carlmontrobotics.lib199.swerve.SwerveModule; @@ -88,11 +89,6 @@ public class Drivetrain extends SubsystemBase { public double extraSpeedMult = 0; private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; - public enum Mode { - coast, - brake, - toggle - } public final Limelight ll; public Drivetrain(Limelight ll) { @@ -146,7 +142,7 @@ public Drivetrain(Limelight ll) { // Supplier pitchSupplier = () -> gyro.getPitch(); // Supplier rollSupplier = () -> gyro.getRoll(); - SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); + SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); driveConfig.openLoopRampRate(secsPer12Volts) .encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing) .velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60) @@ -160,10 +156,11 @@ public Drivetrain(Limelight ll) { .uvwMeasurementPeriod(16); for (int i = 0; i < modules.length; i++) { - modules[i] = new SwerveModule(swerveConfig, SwerveModule.ModuleType.values()[i], - driveMotors[i] = MotorControllerFactory.createSpark(drivePorts[i], driveMotorConfig, driveConfig), - turnMotors[i] = MotorControllerFactory.createSpark(turnPorts[i], turnMotorConfig, turnConfig), - turnEncoders[i] = SensorFactory.createCANCoder(canCoderPorts[i]), i, pitchSupplier, rollSupplier); + modules[i] = new SwerveModule(swerveConfig, SwerveModule.ModuleType.values()[i], + driveMotors[i] = MotorControllerFactory.createSpark(drivePorts[i], driveMotorConfig, driveConfig), + turnMotors[i] = MotorControllerFactory.createSpark(turnPorts[i], turnMotorConfig, turnConfig), + turnEncoders[i] = SensorFactory.createCANCoder(canCoderPorts[i]), + i, pitchSupplier, rollSupplier); } moduleFL = modules[0]; @@ -232,7 +229,11 @@ public void simulationPeriodic() { // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. newAngleDeg -= simGyroOffset.getDegrees(); newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); - gyroYawSim.set(newAngleDeg); + if (gyroYawSim == null) { + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + }else{ + gyroYawSim.set(newAngleDeg); + } } /** @@ -568,22 +569,7 @@ public ChassisSpeeds getSpeeds() { .toArray(SwerveModuleState[]::new)); } - public void setMode(Mode mode) { - for (SwerveModule module : modules){ - switch (mode) { - case coast: - module.coast(); - break; - case brake: - module.brake(); - break; - case toggle: - module.toggleMode(); - break; - } - } - } - /** + /** * Changes between IdleModes */ public void toggleMode() { @@ -601,6 +587,7 @@ public void coast() { module.coast(); } + /** * Sets all SwerveModules to point in a certain angle * @param angle in degrees From 6a8781516ebe26d94c27673a21d48d16a7301200 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 6 Mar 2026 12:42:16 -0800 Subject: [PATCH 09/14] fix dt --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 6317625..b4b9a39 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -9,7 +9,6 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; -import org.carlmontrobotics.lib199.Limelight; import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.SensorFactory; import org.carlmontrobotics.lib199.swerve.SwerveModule; From ee5508cc76b18a3d63f44fc247806a4ebd0062b8 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Tue, 10 Mar 2026 13:00:36 -0700 Subject: [PATCH 10/14] more dt clean up --- .../org/carlmontrobotics/subsystems/Drivetrain.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index b4b9a39..ae59921 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -58,13 +58,12 @@ public class Drivetrain extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator = null; private SwerveModule modules[]; - private String moduleNames[] = {"FL", "FR", "BL", "BR"}; private boolean fieldOriented = true; private double fieldOffset = 0; // FIXME not for permanent use!! - private SparkBase[] driveMotors = new SparkBase[] { null, null, null, null }; - private SparkBase[] turnMotors = new SparkBase[] { null, null, null, null }; - private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; + private SparkBase[] driveMotors = new SparkBase[4]; + private SparkBase[] turnMotors = new SparkBase[4]; + private CANcoder[] turnEncoders = new CANcoder[4]; public final float initPitch; public final float initRoll; @@ -272,9 +271,9 @@ public void initSendable(SendableBuilder builder) { for (SwerveModule module : modules){ SendableRegistry.addChild(this, module); } - for (int i = 0; i < 4; i++) { + for (int i = 0; i < SwerveModule.ModuleType.values().length; i++) { final int j = i; //make java happy - builder.addDoubleProperty(moduleNames[j] + "Turn Encoder (Deg)", () -> modules[j].getModuleAngle(), null); + builder.addDoubleProperty((SwerveModule.ModuleType.values()[j]).toString() + "Turn Encoder (Deg)", () -> modules[j].getModuleAngle(), null); } SendableRegistry.addChild(this, CONFIG); builder.addBooleanProperty("Magnetic Field Disturbance", gyro::isMagneticDisturbance, null); @@ -479,8 +478,10 @@ public void setPose(Pose2d initialPose) { //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); } + //TODO: implement this //This method will set the pose using limelight if it sees a tag and if not it is supposed to run like setPose() public void setPoseWithLimelight(Pose2d backupPose){ //the pose will be set to backupPose if no tag is seen + setPose(backupPose); //FIXME: remove this once we actually implement the method // Rotation2d gyroRotation = gyro.getRotation2d(); // Pose2d pose; From 37cc82f7e6edc04f1912924bd86c0afcd52c0595 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 13 Mar 2026 11:52:51 -0700 Subject: [PATCH 11/14] smartDashbboard --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 6af295f..28a9cca 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -46,6 +46,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -193,6 +194,7 @@ public Drivetrain(Limelight ll) { // Setup autopath builder //configurePPLAutoBuilder(); + SmartDashboard.putData(this); } public boolean isAtAngle(double desiredAngleDeg, double toleranceDeg){ From f231eeb30547739f45f15c63452e7ee3ec3cc2e3 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Sun, 15 Mar 2026 10:30:55 -0700 Subject: [PATCH 12/14] more dt clean up --- .../carlmontrobotics/subsystems/Drivetrain.java | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 28a9cca..d90af43 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -65,8 +65,8 @@ public class Drivetrain extends SubsystemBase { private SparkBase[] driveMotors = new SparkBase[4]; private SparkBase[] turnMotors = new SparkBase[4]; private CANcoder[] turnEncoders = new CANcoder[4]; - public final float initPitch; - public final float initRoll; + public final float initPitch; + public final float initRoll; // debug purposes private SwerveModule moduleFL; @@ -132,16 +132,12 @@ public Drivetrain(Limelight ll) { // Initialize modules { - // initPitch = 0; - // initRoll = 0; Supplier pitchSupplier = () -> 0F; Supplier rollSupplier = () -> 0F; initPitch = gyro.getPitch(); initRoll = gyro.getRoll(); - // Supplier pitchSupplier = () -> gyro.getPitch(); - // Supplier rollSupplier = () -> gyro.getRoll(); - SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); + SparkBaseConfig driveConfig = MotorControllerFactory.sparkConfig(driveMotorConfig); driveConfig.openLoopRampRate(secsPer12Volts) .encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing) .velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60) @@ -172,6 +168,7 @@ public Drivetrain(Limelight ll) { moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() //FIXME this is with values based off of hammerhead }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + simTimer.start(); } for (CANcoder coder : turnEncoders) { @@ -262,7 +259,6 @@ public void setDrivingIdleMode(boolean brake) { @Override public void periodic() { - detectCollision(); //This does nothing PathPlannerLogging.logCurrentPose(getPose()); for (SwerveModule module : modules) { // module.turnPeriodic(); @@ -313,7 +309,7 @@ public void initSendable(SendableBuilder builder) { * positive */ public void setExtraSpeedMult(double set) { - extraSpeedMult=set; + extraSpeedMult = set; } /** @@ -341,7 +337,7 @@ public void drive(SwerveModuleState[] moduleStates) { } /** - * Configures PathPlanner AutoBuilder + * Configures PathPlanner AutoBuilder. */ public void AutoBuilder() { RobotConfig config = Constants.Drivetrainc.Autoc.robotConfig; @@ -426,6 +422,7 @@ private ChassisSpeeds getChassisSpeeds(double forward, double strafe, double rot /** * Constructs and returns four SwerveModuleState objects, one for each side, * using forward, strafe, and rotation values. + * Note: strafe is negated to match the kinematics coordinate convention. * * @param forward The desired forward speed, in m/s. Forward is positive. * @param strafe The desired strafe speed, in m/s. Left is positive. From 7dafcc9e5647ab01be2b210ffb524d213b406b6b Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Sun, 15 Mar 2026 10:37:36 -0700 Subject: [PATCH 13/14] fix some dt bugs --- src/main/java/org/carlmontrobotics/Config.java | 2 +- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index 3d5ec99..17b6ee8 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -22,8 +22,8 @@ public abstract class Config implements Sendable { // Add additional config settings by declaring a protected field, and... protected boolean exampleFlagEnabled = false; - protected boolean vortexDrive = isHammerHead() ? true : false; protected boolean hammerHead = true; + protected boolean vortexDrive = hammerHead; protected boolean setupSysId = false; protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of // outtake through SmartDashboard diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index d90af43..8b4e3ef 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -228,9 +228,8 @@ public void simulationPeriodic() { newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); if (gyroYawSim == null) { gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); - }else{ - gyroYawSim.set(newAngleDeg); } + gyroYawSim.set(newAngleDeg); } /** From 44c940605f72ba067d293f95e2e0091cac615741 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Fri, 17 Apr 2026 16:30:42 -0700 Subject: [PATCH 14/14] autoMaxSpeedMps --- src/main/java/org/carlmontrobotics/Constants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index d6abbba..16e80f7 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -145,6 +145,7 @@ public static final class Drivetrainc { // Angular velocity = Tangential speed / radius public static final double maxRCW = maxSpeed / swerveRadius; + public static final double autoMaxSpeedMps = 2 ; // Meters / second public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 public static final double autoMaxAmps = 40.0; // The maximum acceleration the robot can achieve is equal to the coefficient of