From e5b6e4e3c72e760192e2222ed8d4bb67e64c0d92 Mon Sep 17 00:00:00 2001 From: nharnwal Date: Sat, 27 Jan 2024 08:52:02 -0500 Subject: [PATCH] Closed loop control working, seperate FFC for left and right --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 8 +++-- .../frc/robot/subsystems/drive/Drive.java | 32 +++++++++---------- .../subsystems/drive/DriveIOTalonSRX.java | 31 +++++++----------- 4 files changed, 34 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9f889b7e..9c293117 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,6 +28,7 @@ public final class Constants { public static final double INTAKE_SHOOTER_VOLTS = -8.0; public static final double INTAKE_HOPPER_VOLTS = -4.0; + public static final double TICKS_PER_REVOLUTION = 1440; public static final Mode currentMode = Mode.REAL; public static enum Mode { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a08fa89..6042accb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -81,9 +81,13 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set up feedforward characterization autoChooser.addOption( - "Drive FF Characterization", + "Drive FF Characterization Left", new FeedForwardCharacterization( - drive, (volts) -> drive.driveVolts(volts, volts), drive::getCharacterizationVelocity)); + drive, (volts) -> drive.driveVolts(volts, volts), drive::getLeftVelocityMetersPerSec)); + autoChooser.addOption( + "Drive FF Characterization Right", + new FeedForwardCharacterization( + drive, (volts) -> drive.driveVolts(volts, volts), drive::getRightVelocityMetersPerSec)); autoChooser.addOption( "Flywheel FF Characterization", new FeedForwardCharacterization( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 00b3f736..8ce0fb41 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -43,8 +43,11 @@ public class Drive extends SubsystemBase { // TODO: NON-SIM FEEDFORWARD GAINS MUST BE TUNED // Consider using SysId routines defined in RobotContainer - private static final double KS = Constants.currentMode == Mode.SIM ? 0.0 : 0.0; - private static final double KV = Constants.currentMode == Mode.SIM ? 0.227 : 0.0; + private static final double lKS = Constants.currentMode == Mode.SIM ? 0.0 : 0.90278; + private static final double lKV = Constants.currentMode == Mode.SIM ? 0.227 : 30.62084; + + private static final double rKS = Constants.currentMode == Mode.SIM ? 0.0 : 0.82739; + private static final double rKV = Constants.currentMode == Mode.SIM ? 0.227 : 34.45523; private final PIDController pid = new PIDController(0, 0, 0); @@ -54,7 +57,8 @@ public class Drive extends SubsystemBase { new DifferentialDriveOdometry(new Rotation2d(), 0.0, 0.0); private final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(TRACK_WIDTH); - private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV); + private final SimpleMotorFeedforward feedforwardLeft = new SimpleMotorFeedforward(lKS, lKV); + private final SimpleMotorFeedforward feedforwardRight = new SimpleMotorFeedforward(rKS, rKV); /** Creates a new Drive. */ public Drive(DriveIO io) { @@ -106,22 +110,18 @@ public void driveVolts(double leftVolts, double rightVolts) { public void driveVelocity(double leftMetersPerSec, double rightMetersPerSec) { Logger.recordOutput("Drive/LeftVelocitySetpointMetersPerSec", leftMetersPerSec); Logger.recordOutput("Drive/RightVelocitySetpointMetersPerSec", rightMetersPerSec); - double leftRadPerSec = leftMetersPerSec / WHEEL_RADIUS; - double rightRadPerSec = rightMetersPerSec / WHEEL_RADIUS; - io.setVelocity( - leftRadPerSec, - rightRadPerSec, - feedforward.calculate(leftRadPerSec), - feedforward.calculate(rightRadPerSec)); + + io.setVoltage( + pid.calculate(inputs.leftVelocityRadPerSec * WHEEL_RADIUS, leftMetersPerSec) + + (feedforwardLeft.calculate(leftMetersPerSec)), + pid.calculate(inputs.rightVelocityRadPerSec * WHEEL_RADIUS, rightMetersPerSec) + + (feedforwardRight.calculate(rightMetersPerSec))); } /** Run open loop based on stick positions. */ public void driveArcade(double xSpeed, double zRotation) { var speeds = DifferentialDrive.arcadeDriveIK(xSpeed, zRotation, true); - io.setVoltage( - pid.calculate(inputs.leftVelocityRadPerSec * WHEEL_RADIUS, speeds.left * MAX_SPEED_M_PER_S), - pid.calculate( - inputs.rightVelocityRadPerSec * WHEEL_RADIUS, speeds.right * MAX_SPEED_M_PER_S)); + driveVelocity(speeds.left * MAX_SPEED_M_PER_S, speeds.right * MAX_SPEED_M_PER_S); } /** Stops the drive. */ @@ -164,8 +164,8 @@ public double getRightVelocityMetersPerSec() { return inputs.rightVelocityRadPerSec * WHEEL_RADIUS; } - /** Returns the average velocity in radians/second. */ + /** Returns the average velocity in meters/second. */ public double getCharacterizationVelocity() { - return (inputs.leftVelocityRadPerSec + inputs.rightVelocityRadPerSec) / 2.0; + return (inputs.leftVelocityRadPerSec + inputs.rightVelocityRadPerSec) / 2.0 * WHEEL_RADIUS; } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java b/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java index f6777dd8..9902d547 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java @@ -17,19 +17,19 @@ import static com.ctre.phoenix.motorcontrol.NeutralMode.Brake; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; -import com.ctre.phoenix.motorcontrol.can.SlotConfiguration; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import frc.robot.Constants; public class DriveIOTalonSRX implements DriveIO { - private static final double GEAR_RATIO = 10.0; + private static final double GEAR_RATIO = 10.0; // was 10 private static final double MAX_VOLTAGE = 12.0; private static final double KP = 1.0; // TODO: MUST BE TUNED, consider using Phoenix Tuner X private static final double KD = 0.0; // TODO: MUST BE TUNED, consider using Phoenix Tuner X - private PIDController pid = new PIDController(0.0, 0.0, 0.0); + private PIDController pid = new PIDController(0, 0.0, 0.0); private final TalonSRX leftLeader = new TalonSRX(2); private final TalonSRX leftFollower = new TalonSRX(0); private final TalonSRX rightLeader = new TalonSRX(3); @@ -61,11 +61,6 @@ public DriveIOTalonSRX() { rightLeader.setNeutralMode(Brake); rightFollower.setNeutralMode(Brake); - SlotConfiguration slot = new SlotConfiguration(); - slot.kP = KP; - slot.kD = KD; - config.slot0 = slot; - leftLeader.configAllSettings(config); leftFollower.configAllSettings(config); rightLeader.configAllSettings(config); @@ -94,13 +89,17 @@ public void updateInputs(DriveIOInputs inputs) { rightLeaderCurrent = rightLeader.getStatorCurrent(); rightFollowerCurrent = rightFollower.getStatorCurrent(); - inputs.leftPositionRad = Units.rotationsToRadians(leftPosition) / GEAR_RATIO; - inputs.leftVelocityRadPerSec = Units.rotationsToRadians(leftVelocity) / GEAR_RATIO; + inputs.leftPositionRad = + Units.rotationsToRadians(leftPosition / Constants.TICKS_PER_REVOLUTION); + inputs.leftVelocityRadPerSec = + Units.rotationsToRadians(leftVelocity / Constants.TICKS_PER_REVOLUTION); inputs.leftAppliedVolts = leftAppliedVolts; inputs.leftCurrentAmps = new double[] {leftLeaderCurrent, leftFollowerCurrent}; - inputs.rightPositionRad = Units.rotationsToRadians(rightPosition) / GEAR_RATIO; - inputs.rightVelocityRadPerSec = Units.rotationsToRadians(rightVelocity) / GEAR_RATIO; + inputs.rightPositionRad = + Units.rotationsToRadians(rightPosition / Constants.TICKS_PER_REVOLUTION); + inputs.rightVelocityRadPerSec = + Units.rotationsToRadians(rightVelocity / Constants.TICKS_PER_REVOLUTION); inputs.rightAppliedVolts = rightAppliedVolts; inputs.rightCurrentAmps = new double[] {rightLeaderCurrent, rightFollowerCurrent}; } @@ -110,12 +109,4 @@ public void setVoltage(double leftVolts, double rightVolts) { leftLeader.set(TalonSRXControlMode.PercentOutput, leftVolts / MAX_VOLTAGE); rightLeader.set(TalonSRXControlMode.PercentOutput, -1 * rightVolts / MAX_VOLTAGE); } - - @Override - public void setVelocity( - double leftRadPerSec, double rightRadPerSec, double leftFFVolts, double rightFFVolts) { - setVoltage( - pid.calculate(leftLeader.getSelectedSensorVelocity(), leftRadPerSec) + leftFFVolts, - pid.calculate(rightLeader.getSelectedSensorVelocity(), rightRadPerSec) + rightFFVolts); - } }