From 59607aad654cdb59294f54fffe46b2bcfa99331c Mon Sep 17 00:00:00 2001 From: Austin L Date: Mon, 19 Jan 2026 20:54:15 -0600 Subject: [PATCH 1/4] Added characterization controlls for drive base and characterized. Updated PIDs for drive base Checked over gear ratio in tuner constants. Updated volts for launcher and indexer motor to be able to score. Made center path start and end in the correct oreintation. Reduced speed in all autos to 1 m per s. Updated wheel radius. --- .wpilib/wpilib_preferences.json | 2 +- .../deploy/pathplanner/paths/Center.0.path | 6 +-- .../deploy/pathplanner/paths/Center.1.path | 6 +-- src/main/deploy/pathplanner/paths/Feed.0.path | 4 +- src/main/deploy/pathplanner/paths/Feed.1.path | 4 +- src/main/deploy/pathplanner/settings.json | 21 +++++++-- src/main/java/frc/robot/RobotContainer.java | 47 +++++++++++++++++++ .../frc/robot/commands/DriveCommands.java | 8 ++++ .../frc/robot/generated/TunerConstants.java | 18 +++++-- 9 files changed, 96 insertions(+), 20 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index c127537..06f324c 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2026", - "teamNumber": 9930 + "teamNumber": 9990 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center.0.path b/src/main/deploy/pathplanner/paths/Center.0.path index 07aeb9f..60e1418 100644 --- a/src/main/deploy/pathplanner/paths/Center.0.path +++ b/src/main/deploy/pathplanner/paths/Center.0.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center.1.path b/src/main/deploy/pathplanner/paths/Center.1.path index 1013bf4..cf48b96 100644 --- a/src/main/deploy/pathplanner/paths/Center.1.path +++ b/src/main/deploy/pathplanner/paths/Center.1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": -90.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Feed.0.path b/src/main/deploy/pathplanner/paths/Feed.0.path index 3afd57e..ef30e8f 100644 --- a/src/main/deploy/pathplanner/paths/Feed.0.path +++ b/src/main/deploy/pathplanner/paths/Feed.0.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Feed.1.path b/src/main/deploy/pathplanner/paths/Feed.1.path index bb74391..e13c9f9 100644 --- a/src/main/deploy/pathplanner/paths/Feed.1.path +++ b/src/main/deploy/pathplanner/paths/Feed.1.path @@ -65,8 +65,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f981b63..564bf61 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,18 +4,29 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, "robotMass": 74.088, "robotMOI": 6.883, - "robotWheelbase": 0.546, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60FOC", + "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, - "wheelCOF": 1.2 + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc89419..41583bc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,11 +10,13 @@ import static frc.robot.subsystems.vision.VisionConstants.*; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.goals.RobotGoals; @@ -56,6 +58,7 @@ public class RobotContainer { // Controller private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController characterizeController = new CommandXboxController(4); // Reactive architecture components private final OperatorIntent operatorIntent; @@ -166,6 +169,7 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); + configureCharacterizationButtonBindings(); } /** @@ -208,6 +212,49 @@ private void configureButtonBindings() { // .ignoringDisable(true)); } + public void configureCharacterizationButtonBindings() { + characterizeController + .back() + .and(characterizeController.y()) + .whileTrue(drive.sysIdDynamic(Direction.kForward)); + characterizeController + .back() + .and(characterizeController.x()) + .whileTrue(drive.sysIdDynamic(Direction.kReverse)); + characterizeController + .start() + .and(characterizeController.y()) + .whileTrue(drive.sysIdQuasistatic(Direction.kForward)); + characterizeController + .start() + .and(characterizeController.x()) + .whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); + + characterizeController + .povUp() + .whileTrue(DriveCommands.wheelRadiusCharacterization(drive)) + .onFalse(DriveCommands.brakeDrive(drive)); + + characterizeController + .a() + .onTrue( + new InstantCommand( + () -> { + SignalLogger.setPath("/media/sda1/logs"); + // SignalLogger.enableAutoLogging(true); + SignalLogger.start(); + System.out.println("Started Logger"); + })); + characterizeController + .b() + .onTrue( + new InstantCommand( + () -> { + SignalLogger.stop(); + System.out.println("Stopped Logger"); + })); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a749f60..578bfb8 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.drive.Drive; import java.text.DecimalFormat; import java.text.NumberFormat; @@ -287,4 +288,11 @@ private static class WheelRadiusCharacterizationState { Rotation2d lastAngle = Rotation2d.kZero; double gyroDelta = 0.0; } + + public static Command brakeDrive(Drive drive) { + return new InstantCommand( + () -> { + drive.stop(); + }); + } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index d10ebae..b14304e 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -32,8 +32,14 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - + // new Slot0Configs() + // .withKP(0.49) // 0.2204171275 + // .withKI(0) + // .withKD(0.0) + // .withKS(0.125) // 0.155625 + // .withKV(0.734) // 0.7290475 + // .withKA(0.103); // 0.08876375; + new Slot0Configs().withKP(0.34).withKI(0).withKD(0).withKS(0.22).withKV(0.72).withKA(0.06); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; @@ -85,8 +91,12 @@ public class TunerConstants { private static final double kCoupleRatio = 0; private static final double kDriveGearRatio = 5.8909090909090915; - private static final double kSteerGearRatio = 11.314285714285717; - private static final Distance kWheelRadius = Inches.of(2); + private static final double kSteerGearRatio = 12.1; + // ********** Wheel Radius Characterization Results ********** + // Wheel Delta: 55.224 radians + // Gyro Delta: 6.245 radians + // Wheel Radius: 0.049 meters, 1.943 inches + private static final Distance kWheelRadius = Inches.of(1.943); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 95963d9fed6c261daba435a440d482b8871c3059 Mon Sep 17 00:00:00 2001 From: chasereinders-dot Date: Tue, 20 Jan 2026 18:21:43 -0600 Subject: [PATCH 2/4] changes voltage values in launch command --- src/main/java/frc/robot/RobotContainer.java | 1 + .../java/frc/robot/subsystems/launcher/LauncherSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 41583bc..6685fe8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.DriveCommands; diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java index cc7999d..81f33c6 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java @@ -53,8 +53,8 @@ public void setIndexerSpeed(Voltage speed) { public Command launchCommand() { return runOnce( () -> { - setIndexerSpeed(Volts.of(2)); - setLaunchSpeed(Volts.of(2)); + setIndexerSpeed(Volts.of(4.0)); + setLaunchSpeed(Volts.of(11.0)); }); } From ab668c3d3f60d03475290a77d555748657f3dbe4 Mon Sep 17 00:00:00 2001 From: chasereinders-dot Date: Tue, 20 Jan 2026 18:44:06 -0600 Subject: [PATCH 3/4] fixed stop behavior --- src/main/java/frc/robot/goals/RobotGoalsBehavior.java | 4 ++-- .../java/frc/robot/subsystems/launcher/LauncherSubsystem.java | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/goals/RobotGoalsBehavior.java b/src/main/java/frc/robot/goals/RobotGoalsBehavior.java index 61cc4ca..322b824 100644 --- a/src/main/java/frc/robot/goals/RobotGoalsBehavior.java +++ b/src/main/java/frc/robot/goals/RobotGoalsBehavior.java @@ -25,7 +25,7 @@ public RobotGoalsBehavior(RobotGoals goals) { public void configure(OperatorIntentEvents intent) { intent .wantsToScoreTrigger() - .whileTrue(goals.setGoalCommand(RobotGoal.LAUNCHING)) - .whileFalse(goals.setGoalCommand(RobotGoal.IDLE)); + .onTrue(goals.setGoalCommand(RobotGoal.LAUNCHING)) + .onFalse(goals.setGoalCommand(RobotGoal.IDLE)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java index 81f33c6..223664c 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java @@ -61,8 +61,7 @@ public Command launchCommand() { public Command idleCommand() { return runOnce( () -> { - setIndexerSpeed(Volts.of(0)); - setLaunchSpeed(Volts.of(0)); + stop(); }); } From caaafeefef4b185b9d038e3b611359351223034b Mon Sep 17 00:00:00 2001 From: chasereinders-dot Date: Tue, 20 Jan 2026 19:27:43 -0600 Subject: [PATCH 4/4] Set the log tunable numbers for max speed and slip --- AdvantageScope Swerve Calibration.json | 58 +++++++++---------- .../frc/robot/generated/TunerConstants.java | 5 +- 2 files changed, 32 insertions(+), 31 deletions(-) diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index a84f77f..af9cf59 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -9,11 +9,11 @@ "sidebar": { "width": 300, "expanded": [ - "/Drive", - "/RealOutputs", - "/RealOutputs/SwerveStates", - "/RealOutputs/SwerveChassisSpeeds", - "/RealOutputs/Odometry" + "NT:/AdvantageKit/Drive", + "NT:/AdvantageKit/RealOutputs", + "NT:/AdvantageKit/RealOutputs/SwerveStates", + "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds", + "NT:/AdvantageKit/RealOutputs/Odometry" ] }, "tabs": { @@ -34,7 +34,7 @@ "sources": [ { "type": "states", - "logKey": "/RealOutputs/SwerveStates/Measured", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", "logType": "SwerveModuleState[]", "visible": true, "options": { @@ -44,7 +44,7 @@ }, { "type": "states", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized", "logType": "SwerveModuleState[]", "visible": true, "options": { @@ -54,7 +54,7 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured", "logType": "ChassisSpeeds", "visible": true, "options": { @@ -63,7 +63,7 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Setpoints", "logType": "ChassisSpeeds", "visible": true, "options": { @@ -72,7 +72,7 @@ }, { "type": "rotation", - "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation", "logType": "Rotation2d", "visible": true, "options": {} @@ -94,7 +94,7 @@ "sources": [ { "type": "robot", - "logKey": "/RealOutputs/Odometry/Robot", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot", "logType": "Pose2d", "visible": true, "options": { @@ -103,7 +103,7 @@ }, { "type": "trajectory", - "logKey": "/RealOutputs/Odometry/Trajectory", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Trajectory", "logType": "Pose2d[]", "visible": true, "options": { @@ -113,7 +113,7 @@ }, { "type": "ghost", - "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/TrajectorySetpoint", "logType": "Pose2d", "visible": true, "options": { @@ -148,7 +148,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module0/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -158,7 +158,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module1/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module1/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -168,7 +168,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module2/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module2/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -178,7 +178,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module3/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module3/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -213,7 +213,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module0/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -223,7 +223,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module1/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module1/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -233,7 +233,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module2/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module2/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -243,7 +243,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module3/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module3/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -278,7 +278,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/angle/value", "logType": "Number", "visible": true, "options": { @@ -288,7 +288,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", "logType": "Number", "visible": true, "options": { @@ -323,7 +323,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/speed", "logType": "Number", "visible": true, "options": { @@ -333,7 +333,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", "logType": "Number", "visible": true, "options": { @@ -368,7 +368,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured/vx", "logType": "Number", "visible": true, "options": { @@ -378,7 +378,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured/vy", "logType": "Number", "visible": true, "options": { @@ -413,7 +413,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/DriveCurrentAmps", + "logKey": "NT:/AdvantageKit/Drive/Module0/DriveCurrentAmps", "logType": "Number", "visible": true, "options": { @@ -425,7 +425,7 @@ "rightSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/DriveVelocityRadPerSec", + "logKey": "NT:/AdvantageKit/Drive/Module0/DriveVelocityRadPerSec", "logType": "Number", "visible": true, "options": { diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b14304e..9542cee 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -60,7 +60,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120); + private static final Current kSlipCurrent = Amps.of(46); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -84,7 +84,8 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.24); + // Max theroretical speed 5.4 + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.81); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot