Skip to content

Commit

Permalink
Add motion magic control to kraken and fix units
Browse files Browse the repository at this point in the history
  • Loading branch information
@superpenguin612
superpenguin612 committed Jan 9, 2024
1 parent b75f61d commit 20909ee
Showing 1 changed file with 66 additions and 81 deletions.
147 changes: 66 additions & 81 deletions src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
Expand All @@ -17,12 +20,10 @@
import com.techhounds.houndutil.houndlog.interfaces.LoggedObject;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
Expand All @@ -45,8 +46,8 @@ public class KrakenCoaxialSwerveModule implements CoaxialSwerveModule {
private PIDController drivePidController;

/** The PID controller that controls the steer motor's position. */
@Log
private ProfiledPIDController steerPidController;
// @Log
// private ProfiledPIDController steerPidController;

/** The feedforward controller that controls the drive motor's velocity. */
@Log
Expand All @@ -56,8 +57,6 @@ public class KrakenCoaxialSwerveModule implements CoaxialSwerveModule {
private DCMotorSim driveMotorSim;
@Log
private DCMotorSim steerMotorSim;
@Log
private SwerveModulePosition lastPosition = getPosition();

@Log(groups = "control")
private double driveFeedbackVoltage = 0.0;
Expand All @@ -66,27 +65,11 @@ public class KrakenCoaxialSwerveModule implements CoaxialSwerveModule {
@Log(groups = "control")
private double turnFeedbackVoltage = 0.0;

@Log
private double simDriveEncoderPosition = 0.0;
@Log
private double simDriveEncoderVelocity = 0.0;
@Log
private double simCurrentAngle = 0.0;

private final SwerveConstants SWERVE_CONSTANTS;

/**
* The offset of the CANCoder from the zero point, in radians. This will be
* added to any measurements obtained from the CANCoder.
*/
@Log
private double turnCanCoderOffset;

@Log
private boolean isUsingAbsoluteEncoder;

@Log
private double accel = 0;
private final VoltageOut driveVoltageRequest = new VoltageOut(0);
private final MotionMagicVelocityVoltage driveVelocityRequest = new MotionMagicVelocityVoltage(0);
private final MotionMagicVoltage steerPositionRequest = new MotionMagicVoltage(0);

/**
* Initalizes a SwerveModule.
Expand Down Expand Up @@ -120,14 +103,27 @@ public KrakenCoaxialSwerveModule(
driveConfig.MotorOutput.Inverted = driveMotorInverted ? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
driveConfig.CurrentLimits.StatorCurrentLimit = SWERVE_CONSTANTS.DRIVE_CURRENT_LIMIT;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
driveConfig.Feedback.SensorToMechanismRatio = SWERVE_CONSTANTS.DRIVE_GEARING;
driveConfig.CurrentLimits.SupplyCurrentLimit = SWERVE_CONSTANTS.DRIVE_CURRENT_LIMIT;
driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
driveConfig.CurrentLimits.SupplyCurrentThreshold = 300;
driveConfig.CurrentLimits.SupplyTimeThreshold = 1;
driveConfig.Slot0.kS = SWERVE_CONSTANTS.DRIVE_kS;
driveConfig.Slot0.kV = SWERVE_CONSTANTS.DRIVE_kV;
driveConfig.Slot0.kP = SWERVE_CONSTANTS.DRIVE_kP;
driveConfig.Slot0.kI = SWERVE_CONSTANTS.DRIVE_kI;
driveConfig.Slot0.kD = SWERVE_CONSTANTS.DRIVE_kD;
driveConfig.MotionMagic.MotionMagicCruiseVelocity = SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND
/ SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE;
driveConfig.MotionMagic.MotionMagicAcceleration = SWERVE_CONSTANTS.MAX_DRIVING_ACCELERATION_METERS_PER_SECOND_SQUARED
/ SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE;
driveConfigurator.apply(driveConfig);

steerCanCoder = new CANcoder(canCoderChannel);
MagnetSensorConfigs config = new MagnetSensorConfigs();
config.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
config.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
config.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
config.MagnetOffset = steerCanCoderOffset;
steerCanCoder.getConfigurator().apply(config);

steerMotor = new TalonFX(steerMotorChannel, canBus);
Expand All @@ -140,19 +136,25 @@ public KrakenCoaxialSwerveModule(
steerConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
steerConfig.Feedback.SensorToMechanismRatio = 1.0;
steerConfig.Feedback.RotorToSensorRatio = SWERVE_CONSTANTS.STEER_GEARING;
steerConfig.CurrentLimits.StatorCurrentLimit = SWERVE_CONSTANTS.STEER_CURRENT_LIMIT;
steerConfig.CurrentLimits.StatorCurrentLimitEnable = true;
steerConfig.CurrentLimits.SupplyCurrentLimit = SWERVE_CONSTANTS.STEER_CURRENT_LIMIT;
steerConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
steerConfig.CurrentLimits.SupplyCurrentThreshold = 300;
steerConfig.CurrentLimits.SupplyTimeThreshold = 1;

steerConfig.Slot0.kP = SWERVE_CONSTANTS.STEER_kP;
steerConfig.Slot0.kI = SWERVE_CONSTANTS.STEER_kI;
steerConfig.Slot0.kD = SWERVE_CONSTANTS.STEER_kD;
steerConfig.MotionMagic.MotionMagicCruiseVelocity = SWERVE_CONSTANTS.MAX_STEER_VELOCITY_RADIANS_PER_SECOND
/ (2 * Math.PI);
steerConfig.MotionMagic.MotionMagicAcceleration = SWERVE_CONSTANTS.MAX_STEER_ACCELERATION_RADIANS_PER_SECOND_SQUARED
/ (2 * Math.PI);
steerConfig.ClosedLoopGeneral.ContinuousWrap = true;
steerConfigurator.apply(steerConfig);

drivePidController = new PIDController(SWERVE_CONSTANTS.DRIVE_kP,
SWERVE_CONSTANTS.DRIVE_kI,
SWERVE_CONSTANTS.DRIVE_kD);
steerPidController = new ProfiledPIDController(
SWERVE_CONSTANTS.STEER_kP, SWERVE_CONSTANTS.STEER_kI,
SWERVE_CONSTANTS.STEER_kD,
new TrapezoidProfile.Constraints(
SWERVE_CONSTANTS.MAX_STEER_VELOCITY_RADIANS_PER_SECOND,
SWERVE_CONSTANTS.MAX_STEER_ACCELERATION_RADIANS_PER_SECOND_SQUARED));

driveFeedforward = new SimpleMotorFeedforward(SWERVE_CONSTANTS.DRIVE_kS,
SWERVE_CONSTANTS.DRIVE_kV,
SWERVE_CONSTANTS.DRIVE_kA);
Expand All @@ -164,41 +166,33 @@ public KrakenCoaxialSwerveModule(
SWERVE_CONSTANTS.STEER_GEARING,
SWERVE_CONSTANTS.STEER_MOI);

steerPidController.enableContinuousInput(0, 2 * Math.PI);
// steerPidController.enableContinuousInput(0, 2 * Math.PI);

if (RobotBase.isSimulation()) {
this.simDriveEncoderPosition = 0.0;
this.simDriveEncoderVelocity = 0.0;
this.simCurrentAngle = 0.0;
}
// if (RobotBase.isSimulation()) {
// this.simDriveEncoderPosition = 0.0;
// this.simDriveEncoderVelocity = 0.0;
// this.simCurrentAngle = 0.0;
// }
}

@Override
public double getDriveMotorPosition() {
if (RobotBase.isReal())
return driveMotor.getPosition().getValue();
else
return simDriveEncoderPosition;
return driveMotor.getPosition().getValue() * SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE;
}

@Override
public double getDriveMotorVelocity() {
if (RobotBase.isReal())
return driveMotor.getVelocity().getValue();
else
return simDriveEncoderVelocity;
return driveMotor.getVelocity().getValue();
}

@Override
public double getDriveMotorVoltage() {
return driveMotor.getMotorVoltage().getValue();
}

@Override
public Rotation2d getWheelAngle() {
if (RobotBase.isReal())
return new Rotation2d(
this.isUsingAbsoluteEncoder
? Units.rotationsToRadians(steerCanCoder.getPosition().getValue()) +
turnCanCoderOffset
: steerMotor.getPosition().getValue());
else
return new Rotation2d(simCurrentAngle);
return new Rotation2d(Units.rotationsToRadians(steerMotor.getPosition().getValue()));
}

@Override
Expand Down Expand Up @@ -232,7 +226,7 @@ public void setMotorHoldMode(MotorHoldMode motorHoldMode) {
public void setDriveCurrentLimit(int currentLimit) {
CurrentLimitsConfigs currentConfigs = new CurrentLimitsConfigs();
driveMotor.getConfigurator().refresh(currentConfigs);
currentConfigs.StatorCurrentLimit = currentLimit;
currentConfigs.SupplyCurrentLimit = currentLimit;
currentConfigs.StatorCurrentLimitEnable = true;
driveMotor.getConfigurator().apply(currentConfigs);
}
Expand All @@ -245,38 +239,29 @@ public void stop() {

private void setStateInternal(SwerveModuleState state, boolean openLoop) {
if (openLoop) {
driveMotor.setVoltage(state.speedMetersPerSecond
/ SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND * 12.0);
driveMotor.setControl(driveVoltageRequest.withOutput(
state.speedMetersPerSecond / SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND * 12.0));
} else {
this.driveFeedbackVoltage = drivePidController.calculate(getDriveMotorVelocity(),
state.speedMetersPerSecond);
this.driveFeedforwardVoltage = driveFeedforward.calculate(state.speedMetersPerSecond);

driveMotor.setVoltage(driveFeedbackVoltage + driveFeedforwardVoltage);
driveMotor.setControl(driveVelocityRequest
.withVelocity(state.speedMetersPerSecond / SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE));
}

this.turnFeedbackVoltage = steerPidController.calculate(getWheelAngle().getRadians(),
state.angle.getRadians());

steerMotor.setVoltage(turnFeedbackVoltage);
steerMotor.setControl(steerPositionRequest.withPosition(state.angle.getRotations()));

if (RobotBase.isSimulation()) {
driveMotorSim.setInputVoltage(
driveMotor.getMotorVoltage().getValue() > 12 ? 12 : driveMotor.getMotorVoltage().getValue());
driveMotorSim.setInputVoltage(driveMotor.getSimState().getMotorVoltage());
driveMotorSim.update(0.020);

simDriveEncoderVelocity = driveMotorSim.getAngularVelocityRPM() / 60.0
* SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE;
simDriveEncoderPosition = driveMotorSim.getAngularPositionRotations()
* SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE;
driveMotor.getSimState()
.setRotorVelocity(driveMotorSim.getAngularVelocityRPM() * SWERVE_CONSTANTS.DRIVE_GEARING / 60.0);
driveMotor.getSimState()
.setRawRotorPosition(driveMotorSim.getAngularPositionRotations() * SWERVE_CONSTANTS.DRIVE_GEARING);

steerMotorSim.setInputVoltage(
steerMotor.getMotorVoltage().getValue() > 12 ? 12 : steerMotor.getMotorVoltage().getValue());
steerMotorSim.setInputVoltage(steerMotor.getSimState().getMotorVoltage());
steerMotorSim.update(0.020);

simCurrentAngle = steerMotorSim.getAngularPositionRad();

steerCanCoder.getSimState().setRawPosition(simCurrentAngle);
steerMotor.getSimState().setRawRotorPosition(steerMotorSim.getAngularPositionRotations());
steerCanCoder.getSimState().setRawPosition(steerMotorSim.getAngularPositionRotations());
}
}

Expand Down

0 comments on commit 20909ee

Please sign in to comment.