Skip to content

Commit

Permalink
IMMA SWERVE THE CORNER WOAH OH OH
Browse files Browse the repository at this point in the history
  • Loading branch information
@bbdriverstation
bbdriverstation committed Feb 28, 2025
1 parent 6587c05 commit f2f64d1
Show file tree
Hide file tree
Showing 6 changed files with 85 additions and 69 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/constants/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final Mode currentMode = Mode.SIM;
public static final Mode currentMode = Mode.REAL;

public static enum Mode {
/**
Expand Down
38 changes: 19 additions & 19 deletions src/main/java/frc/robot/constants/DriveConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,30 +47,30 @@ public class DriveConstants {
};

// Zeroed rotation values for each module, see setup instructions
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(-2.0612513383);
public static final Rotation2d frontRightZeroRotation = new Rotation2d(-0.7579322);
public static final Rotation2d backLeftZeroRotation = new Rotation2d(-0.83840501);
public static final Rotation2d backRightZeroRotation = new Rotation2d(1.1981722);
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(Math.PI - 3.136);
public static final Rotation2d frontRightZeroRotation = new Rotation2d(Math.PI -0.389);
public static final Rotation2d backLeftZeroRotation = new Rotation2d(Math.PI - 2.354);
public static final Rotation2d backRightZeroRotation = new Rotation2d(Math.PI - 2.014);

// Device CAN IDs
public static final int pigeonCanId = 11;

public static final int frontLeftDriveCanId = 6;
public static final int backLeftDriveCanId = 7;
public static final int frontRightDriveCanId = 8;
public static final int backRightDriveCanId = 9;
public static final int frontLeftDriveCanId = 8;
public static final int backLeftDriveCanId = 9;
public static final int frontRightDriveCanId = 6;
public static final int backRightDriveCanId = 7;

public static final int frontLeftTurnCanId = 2;
public static final int backLeftTurnCanId = 3;
public static final int frontRightTurnCanId = 5;
public static final int backRightTurnCanId = 4;
public static final int frontLeftTurnCanId = 5;
public static final int backLeftTurnCanId = 4;
public static final int frontRightTurnCanId = 2;
public static final int backRightTurnCanId = 3;

// Encoder Ports

public static final int frontLeftEncoderPort = 3;
public static final int frontRightEncoderPort = 2;
public static final int backLeftEncoderPort = 0;
public static final int backRightEncoderPort = 1;
public static final int frontLeftEncoderPort = 2;
public static final int frontRightEncoderPort = 3;
public static final int backLeftEncoderPort = 1;
public static final int backRightEncoderPort = 0;


// Drive motor configuration
Expand Down Expand Up @@ -105,14 +105,14 @@ public class DriveConstants {
// Turn encoder configuration
public static final double turnEncoderPositionFactor = 2 * Math.PI / turnMotorReduction; // Rotations -> Radians
public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
public static final boolean turnEncoderInverted = false;
public static final boolean turnEncoderInverted = true;

// Turn PID configuration
public static final double turnKp = 20.0;
public static final double turnKp = 10.0;
public static final double turnKd = 0.0;
public static final double turnSimP = 8.0;
public static final double turnSimD = 0.0;
public static final double turnPIDMinInput = - Math.PI; // Radians
public static final double turnPIDMinInput = -Math.PI; // Radians
public static final double turnPIDMaxInput = Math.PI; // Radians

public static final double robotMassKg = 105.00;
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/subsystems/DriveSubsystem/DriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,7 +266,9 @@ public void runCharacterization(double output) {

/** Stops the drive. */
public void stop() {
runVelocity(new ChassisSpeeds());
for (int i = 0; i < 4; i++) {
modules[i].stop();
}
}

/**
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/subsystems/DriveSubsystem/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,7 @@ public void periodic() {
// Calculate positions for odometry
// int sampleCount = inputs.odometryPhoenixTimestamps.length; // All signals are sampled together

// int sampleCount = Math.min(inputs.odometryPhoenixTimestamps.length, inputs.odometrySparkTimestamps.length);
int sampleCount = 1;
int sampleCount = Math.min(inputs.odometryPhoenixTimestamps.length, inputs.odometrySparkTimestamps.length);
// System.out.println(inputs.odometrySparkTimestamps.length + " " + inputs.odometryPhoenixTimestamps.length);
odometryPositions = new SwerveModulePosition[sampleCount];
for (int i = 0; i < sampleCount; i++) {
Expand Down
99 changes: 53 additions & 46 deletions src/main/java/frc/robot/subsystems/DriveSubsystem/ModuleIOHybrid.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.AnalogInput;
import frc.robot.util.SparkUtil;
import org.littletonrobotics.junction.Logger;

import java.util.Queue;
import java.util.function.DoubleSupplier;
Expand All @@ -52,18 +53,19 @@
*/
public class ModuleIOHybrid implements ModuleIO {
private final Rotation2d zeroRotation;
private final int id;
private final SwerveModuleConstants<
TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
constants;
// Hardware objects
protected final TalonFX driveTalon;
private final SparkBase turnSpark;
// private final ThriftyEncoder turnEncoder;
private final RelativeEncoder turnEncoder;
private final ThriftyEncoder turnEncoder;
// private final RelativeEncoder turnEncoder;

// Closed loop controllers
private final SparkClosedLoopController turnController;
// private final PIDController turnController;
// private final SparkClosedLoopController turnController;
private final PIDController turnController;

// Queue inputs from odometry thread
private final Queue<Double> timestampSparkQueue;
Expand Down Expand Up @@ -98,6 +100,7 @@ public class ModuleIOHybrid implements ModuleIO {

public ModuleIOHybrid(int module, SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants) {
this.constants = constants;
id = module;
zeroRotation =
switch (module) {
case 0 -> frontLeftZeroRotation;
Expand Down Expand Up @@ -166,33 +169,31 @@ public ModuleIOHybrid(int module, SwerveModuleConstants<TalonFXConfiguration, Ta
// Configure turn motor
var turnConfig = new SparkMaxConfig();
turnConfig
.inverted(turnInverted)
.inverted(true)
.idleMode(IdleMode.kBrake)
.smartCurrentLimit(turnMotorCurrentLimit)
.voltageCompensation(12.0);
turnConfig
.encoder
.positionConversionFactor(turnEncoderPositionFactor)
.velocityConversionFactor(turnEncoderVelocityFactor)
.uvwMeasurementPeriod(10)
.uvwAverageDepth(2);
turnConfig
.signals
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
.primaryEncoderVelocityAlwaysOn(true)
.primaryEncoderVelocityPeriodMs(20)
.appliedOutputPeriodMs(20)
.busVoltagePeriodMs(20)
.outputCurrentPeriodMs(20);
turnConfig
.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
.positionWrappingEnabled(true)
.positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput)
// .dFilter(0.4)
.minOutput(0)
.pidf(turnKp, 0.0, turnKd, 0);
// turnConfig
// .encoder
// .positionConversionFactor(turnEncoderPositionFactor)
// .velocityConversionFactor(turnEncoderVelocityFactor)
// .uvwMeasurementPeriod(10)
// .uvwAverageDepth(2);
// turnConfig
// .signals
// .primaryEncoderPositionAlwaysOn(true)
// .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
// .primaryEncoderVelocityAlwaysOn(true)
// .primaryEncoderVelocityPeriodMs(20)
// .appliedOutputPeriodMs(20)
// .busVoltagePeriodMs(20)
// .outputCurrentPeriodMs(20);
// turnConfig
// .closedLoop
// .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
// .positionWrappingEnabled(true)
// .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput)
// .pidf(turnKp, 0.0, turnKd, 0);

SparkUtil.tryUntilOk(
turnSpark,
Expand All @@ -201,8 +202,8 @@ public ModuleIOHybrid(int module, SwerveModuleConstants<TalonFXConfiguration, Ta
turnSpark.configure(
turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));

turnEncoder = turnSpark.getEncoder();
turnController = turnSpark.getClosedLoopController();
// turnEncoder = turnSpark.getEncoder();
// turnController = turnSpark.getClosedLoopController();

int turnAbsoluteEncoderID =
switch (module) {
Expand All @@ -213,16 +214,18 @@ public ModuleIOHybrid(int module, SwerveModuleConstants<TalonFXConfiguration, Ta
default -> 0;
};

ThriftyEncoder thrifty = new ThriftyEncoder(new AnalogInput(turnAbsoluteEncoderID));
SparkUtil.tryUntilOk(turnSpark, 5, () -> turnEncoder.setPosition(thrifty.getRadians()));

turnEncoder = new ThriftyEncoder(new AnalogInput(turnAbsoluteEncoderID));
// SparkUtil.tryUntilOk(turnSpark, 5, () -> turnEncoder.setPosition(thrifty.getRadians() - zeroRotation.getRadians()));
turnController = new PIDController(turnKp, 0, turnKd);
turnController.enableContinuousInput(turnPIDMinInput,turnPIDMaxInput);


// Create odometry queues
timestampSparkQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
turnPositionQueue =
SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition);

turnPositionQueue =
SparkOdometryThread.getInstance().registerSignal(turnEncoder::getPosition);

}

Expand All @@ -232,7 +235,7 @@ public void updateInputs(ModuleIOInputs inputs) {
var driveStatus =
BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);

// turnEncoder.periodic();
turnEncoder.periodic();

// if v = 0 lol
// inputs.turnEncoderConnected = turnEncoder.getConnected();
Expand Down Expand Up @@ -276,8 +279,6 @@ public void updateInputs(ModuleIOInputs inputs) {
.map((Double value) -> new Rotation2d(value).minus(zeroRotation))
.toArray(Rotation2d[]::new);

// inputs.odometryTurnPositions = new Rotation2d[1];
// inputs.odometryTurnPositions[0] = inputs.turnPosition;

timestampSparkQueue.clear();
timestampPhoenixQueue.clear();
Expand Down Expand Up @@ -311,16 +312,22 @@ public void setDriveVelocity(double velocityRadPerSec) {

@Override
public void setTurnPosition(Rotation2d rotation) {
// double range = turnPIDMaxInput - turnPIDMinInput;
// double setpoint = rotation.getRadians() % range;
// double output = turnController.calculate(currentTurnPosition.getRadians(), setpoint);
// System.out.println(setpoint + " " + currentTurnPosition.getRadians());
// setTurnOpenLoop(output);

double setpoint =
double setpoint = rotation.getRadians();
double turnPos =
MathUtil.inputModulus(
rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput);
turnController.setReference(setpoint, ControlType.kPosition);
turnEncoder.getPosition() - zeroRotation.getRadians(), turnPIDMinInput, turnPIDMaxInput);

turnController.enableContinuousInput(turnPIDMinInput, turnPIDMaxInput);
double output = turnController.calculate(turnPos, setpoint);
setTurnOpenLoop(output);

Logger.recordOutput("Module" + id + "/output", output);
Logger.recordOutput("Module" + id + "/setpoint", setpoint);
Logger.recordOutput("Module" + id + "/position", turnPos);
//
// turnController.setReference(setpoint, ControlType.kPosition);


}

}
8 changes: 8 additions & 0 deletions src/main/java/frc/robot/subsystems/DriveSubsystem/ThriftyEncoder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,14 @@ public double getRadians() {
return Units.rotationsToRadians(getRotations());
}

public double getPosition() {
return getRadians();
}

public double getVelocity() {
return getRadiansPerSeconds();
}

public double getRadiansPerSeconds() {
return Units.rotationsToRadians(getRotationsPerSeconds());
}
Expand Down

0 comments on commit f2f64d1

Please sign in to comment.