Skip to content

Commit

Permalink
Revert "Make MAXSwereModule better (#124)"
Browse files Browse the repository at this point in the history 
This reverts commit 68f43a7, reversing
changes made to 5da8c19.
  • Loading branch information
@Oliver-Cushman
Oliver-Cushman committed Feb 3, 2024
1 parent 68f43a7 commit 00d9a5a
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 28 deletions.
9 changes: 7 additions & 2 deletions src/main/java/frc/robot/util/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -313,11 +313,16 @@ public static final class ModuleConstants {
public static final double TURNING_ENCODER_POSITION_PID_MIN_INPUT = 0; // radians
public static final double TURNING_ENCODER_POSITION_PID_MAX_INPUT = TURNING_ENCODER_POSITION_FACTOR; // radians

public static final PIDConstants DRIVING_PID = new PIDConstants(0.04, 0, 0);
public static final double DRIVING_P = 0.04;
public static final double DRIVING_I = 0;
public static final double DRIVING_D = 0;
public static final double DRIVING_FF = 1 / DRIVE_WHEEL_FREE_SPEED_RPS;
public static final double DRIVING_MIN_OUTPUT = -1;
public static final double DRIVING_MAX_OUTPUT = 1;
public static final PIDConstants TURNING_PID = new PIDConstants(Robot.isSimulation() ? 0.5 : 1, 0, 0);

public static final double TURNING_P = Robot.isSimulation() ? 0.5 : 1;
public static final double TURNING_I = 0;
public static final double TURNING_D = 0;
public static final double TURNING_FF = 0;
public static final double TURNING_MIN_OUTPUT = -1;
public static final double TURNING_MAX_OUTPUT = 1;
Expand Down
116 changes: 91 additions & 25 deletions src/main/java/frc/robot/util/MAXSwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,20 +4,29 @@

package frc.robot.util;

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkBase;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkAbsoluteEncoder.Type;
import com.revrobotics.SparkPIDController;
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.wpilibj.Timer;
import frc.robot.util.Constants.FieldConstants;
import frc.robot.util.Constants.ModuleConstants;
import frc.robot.util.Neo.TelemetryPreference;

public class MAXSwerveModule {
private final Neo drivingSpark;
private final Neo turningSpark;

private final RelativeEncoder drivingEncoder;
private final AbsoluteEncoder turningEncoder;

private final SparkPIDController drivingPIDController;
private final SparkPIDController turningPIDController;

private double chassisAngularOffset = 0;
private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d());

Expand All @@ -29,16 +38,18 @@ public class MAXSwerveModule {
*/
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
drivingSpark = new Neo(drivingCANId);
// Invert the turning encoder, since the output shaft rotates in the opposite
// direction of
// the steering motor in the MAXSwerve Module.
turningSpark = new Neo(turningCANId, true);
turningSpark = new Neo(turningCANId);

drivingEncoder = drivingSpark.getEncoder();
turningEncoder = turningSpark.getAbsoluteEncoder(Type.kDutyCycle);
drivingPIDController = drivingSpark.getPIDController();
turningPIDController = turningSpark.getPIDController();

configMotors();

this.chassisAngularOffset = chassisAngularOffset;
desiredState.angle = new Rotation2d(turningSpark.getPosition());
drivingSpark.resetEncoder(0);
desiredState.angle = new Rotation2d(turningEncoder.getPosition());
drivingEncoder.setPosition(0);
}

/**
Expand All @@ -47,6 +58,18 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular
* @return The current state of the module.
*/
public SwerveModuleState getState() {
if (FieldConstants.IS_SIMULATION) {
return getSimState();
}
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModuleState(drivingEncoder.getVelocity(),
new Rotation2d(turningEncoder.getPosition() - chassisAngularOffset));
}

public SwerveModuleState getSimState() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModuleState(drivingSpark.getVelocity(),
new Rotation2d(turningSpark.getPosition() - chassisAngularOffset));

Expand All @@ -58,6 +81,17 @@ public SwerveModuleState getState() {
* @return The current position of the module.
*/
public SwerveModulePosition getPosition() {
if (FieldConstants.IS_SIMULATION) {
return getSimPosition();
}
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModulePosition(
drivingEncoder.getPosition(),
new Rotation2d(turningEncoder.getPosition() - chassisAngularOffset));
}

public SwerveModulePosition getSimPosition() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModulePosition(
Expand All @@ -78,20 +112,27 @@ public void setDesiredState(SwerveModuleState desiredState) {

// Optimize the reference state to avoid spinning further than 90 degrees.
SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctedDesiredState,
new Rotation2d(turningSpark.getPosition()));
new Rotation2d(turningEncoder.getPosition()));

// Command driving and turning SPARKS MAX towards their respective setpoints.
drivingSpark.setTargetVelocity(optimizedDesiredState.speedMetersPerSecond);
turningSpark.setTargetPosition(optimizedDesiredState.angle.getRadians());

if (FieldConstants.IS_SIMULATION) {
drivingSpark.setTargetVelocity(correctedDesiredState.speedMetersPerSecond);
turningSpark.setTargetPosition(correctedDesiredState.angle.getRadians());
} else {
turningPIDController.setReference(optimizedDesiredState.angle.getRadians(),
CANSparkBase.ControlType.kPosition);
drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond,
CANSparkBase.ControlType.kVelocity);
}

this.desiredState = desiredState;
}

/**
* Zeroes all the SwerveModule encoders.
*/
public void resetEncoders() {
drivingSpark.resetEncoder(0);
drivingEncoder.setPosition(0);
}

/**
Expand All @@ -111,21 +152,29 @@ public void setBrakeMode() {
}

public void configMotors() {
turningSpark.restoreFactoryDefaults();
drivingSpark.restoreFactoryDefaults();
Timer.delay(0.25);
// Setup encoders and PID controllers for the driving and turning SPARKS MAX.
SparkPIDController drivingPIDController = drivingSpark.getPIDController();
SparkPIDController turningPIDController = turningSpark.getPIDController();
drivingPIDController.setFeedbackDevice(drivingEncoder);
turningPIDController.setFeedbackDevice(turningEncoder);

// Apply position and velocity conversion factors for the driving encoder. The
// native units for position and velocity are rotations and RPM, respectively,
// but we want meters and meters per second to use with WPILib's swerve APIs.
drivingSpark.setPositionConversionFactor(ModuleConstants.DRIVING_ENCODER_POSITION_FACTOR);
drivingSpark.setVelocityConversionFactor(ModuleConstants.DRIVING_ENCODER_VELOCITY_FACTOR);
drivingEncoder.setPositionConversionFactor(ModuleConstants.DRIVING_ENCODER_POSITION_FACTOR);
drivingEncoder.setVelocityConversionFactor(ModuleConstants.DRIVING_ENCODER_VELOCITY_FACTOR);

// Apply position and velocity conversion factors for the turning encoder. We
// want these in radians and radians per second to use with WPILib's swerve
// APIs.
turningSpark.setPositionConversionFactor(ModuleConstants.TURNING_ENCODER_POSITION_FACTOR);
turningSpark.setVelocityConversionFactor(ModuleConstants.TURNING_ENCODER_VELOCITY_FACTOR);
turningEncoder.setPositionConversionFactor(ModuleConstants.TURNING_ENCODER_POSITION_FACTOR);
turningEncoder.setVelocityConversionFactor(ModuleConstants.TURNING_ENCODER_VELOCITY_FACTOR);

// Invert the turning encoder, since the output shaft rotates in the opposite
// direction of
// the steering motor in the MAXSwerve Module.
turningEncoder.setInverted(ModuleConstants.TURNING_ENCODER_INVERTED);

// Enable PID wrap around for the turning motor. This will allow the PID
// controller to go through 0 to get to the setpoint i.e. going from 350 degrees
Expand All @@ -135,18 +184,35 @@ public void configMotors() {
turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.TURNING_ENCODER_POSITION_PID_MIN_INPUT);
turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.TURNING_ENCODER_POSITION_PID_MAX_INPUT);

// Set the PID gains for the driving motor
// it needs to be tuned every year for a new robot
drivingSpark.setPID(ModuleConstants.DRIVING_PID);
turningSpark.setPID(ModuleConstants.TURNING_PID);
// Set the PID gains for the driving motor. Note these are example gains, and
// you
// may need to tune them for your own robot!
drivingPIDController.setP(ModuleConstants.DRIVING_P);
drivingPIDController.setI(ModuleConstants.DRIVING_I);
drivingPIDController.setD(ModuleConstants.DRIVING_D);
drivingPIDController.setFF(ModuleConstants.DRIVING_FF);
drivingPIDController.setOutputRange(ModuleConstants.DRIVING_MIN_OUTPUT,
ModuleConstants.DRIVING_MAX_OUTPUT);

// Set the PID gains for the turning motor. Note these are example gains, and
// you
// may need to tune them for your own robot!
turningPIDController.setP(ModuleConstants.TURNING_P);
turningPIDController.setI(ModuleConstants.TURNING_I);
turningPIDController.setD(ModuleConstants.TURNING_D);
turningPIDController.setFF(ModuleConstants.TURNING_FF);
turningPIDController.setOutputRange(ModuleConstants.TURNING_MIN_OUTPUT,
ModuleConstants.TURNING_MAX_OUTPUT);

drivingSpark.setIdleMode(CANSparkBase.IdleMode.kBrake);
turningSpark.setIdleMode(CANSparkBase.IdleMode.kBrake);
drivingSpark.setSmartCurrentLimit(ModuleConstants.VORTEX_CURRENT_LIMIT);
turningSpark.setSmartCurrentLimit(ModuleConstants.TURNING_MOTOR_CURRENT_LIMIT);

// See https://docs.revrobotics.com/Spark/operating-modes/control-interfaces#periodic-status-5-default-rate-200ms
drivingSpark.setTelemetryPreference(TelemetryPreference.ONLY_RELATIVE_ENCODER);
turningSpark.setTelemetryPreference(TelemetryPreference.ONLY_ABSOLUTE_ENCODER);
// See
// https://docs.revrobotics.com/Spark/operating-modes/control-interfaces#periodic-status-5-default-rate-200ms
drivingSpark.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
turningSpark.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
turningSpark.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/util/Neo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -306,7 +306,7 @@ public double getVelocity() {
*
* @param position the desired position to set
*/
public void resetEncoder(double position) {
public void setPosition(double position) {
encoder.setPosition(position * reversedMultiplier);
}

Expand Down

0 comments on commit 00d9a5a

Please sign in to comment.