Skip to content

Commit

Permalink
Z Willy is the best (on Z Willy)
Browse files Browse the repository at this point in the history
  • Loading branch information
@GreenTomato5
GreenTomato5 committed Nov 19, 2024
1 parent 76140c1 commit 7a84cd2
Show file tree
Hide file tree
Showing 8 changed files with 40 additions and 35 deletions.
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,22 +174,22 @@ public static final class Real {
new SwerveModule(
new SwerveMotorIOTalonFX(59, CAN_BUS, DRIVE_BASE.driveGearing),
new SwerveMotorIOTalonFX(8, CAN_BUS, DRIVE_BASE.azimuthGearing),
new SwerveAbsoluteEncoderIOCANcoder(3, 0),
new SwerveAbsoluteEncoderIOCANcoder(3, Units.radiansToDegrees(0.206)),
"FrontLeft"),
new SwerveModule(
new SwerveMotorIOTalonFX(56, CAN_BUS, DRIVE_BASE.driveGearing),
new SwerveMotorIOTalonFX(4, CAN_BUS, DRIVE_BASE.azimuthGearing),
new SwerveAbsoluteEncoderIOCANcoder(6, 0),
new SwerveAbsoluteEncoderIOCANcoder(6, -Units.radiansToDegrees(2.772)),
"FrontRight"),
new SwerveModule(
new SwerveMotorIOTalonFX(11, CAN_BUS, DRIVE_BASE.driveGearing),
new SwerveMotorIOTalonFX(2, CAN_BUS, DRIVE_BASE.azimuthGearing),
new SwerveAbsoluteEncoderIOCANcoder(12, 0),
new SwerveAbsoluteEncoderIOCANcoder(12, Units.radiansToDegrees(2.398)),
"BackLeft"),
new SwerveModule(
new SwerveMotorIOTalonFX(58, CAN_BUS, DRIVE_BASE.driveGearing),
new SwerveMotorIOTalonFX(5, CAN_BUS, DRIVE_BASE.azimuthGearing),
new SwerveAbsoluteEncoderIOCANcoder(9, 0),
new SwerveAbsoluteEncoderIOCANcoder(9, -Units.radiansToDegrees(1.934)),
"BackRight")
};
}
Expand Down
43 changes: 22 additions & 21 deletions ...rc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,43 +13,44 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Unit;
import edu.wpi.first.wpilibj.CAN;

public class SwerveAbsoluteEncoderIOCANcoder implements SwerveAbsoluteEncoderIO {

private double absoluteEncoderOffset;
private boolean inverted;

// private StatusSignal<Double> turnAbsolutePosition;
private StatusSignal<Double> turnAbsolutePosition;
private CANcoderConfigurator configurator;
private CANcoder CaNcoder;
private MagnetSensorConfigs magnetSensorConfiguration;

public SwerveAbsoluteEncoderIOCANcoder(int ID, double encoderOffset) {
this.absoluteEncoderOffset = encoderOffset;
this.inverted = false;

double offsetRotations = encoderOffset/360;

CaNcoder = new CANcoder(ID);
// this.turnAbsolutePosition = CaNcoder.getAbsolutePosition();
// this.configurator = CaNcoder.getConfigurator();

// this.magnetSensorConfiguration = new MagnetSensorConfigs();
this.turnAbsolutePosition = CaNcoder.getAbsolutePosition();
this.configurator = CaNcoder.getConfigurator();

// configurator.apply(new CANcoderConfiguration());
this.magnetSensorConfiguration = new MagnetSensorConfigs();

// configurator.refresh(magnetSensorConfiguration);
// configurator.apply(magnetSensorConfiguration
// .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
// .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
// );
// configurator.apply(magnetSensorConfiguration.withMagnetOffset(encoderOffset/360));
// CaNcoder.getConfigurator().withSensorDirection(getS)

configurator.apply(new CANcoderConfiguration());

configurator.refresh(magnetSensorConfiguration);
configurator.apply(magnetSensorConfiguration
.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
);
configurator.apply(magnetSensorConfiguration.withSensorDirection(SensorDirectionValue.CounterClockwise_Positive));
configurator.apply(magnetSensorConfiguration.withMagnetOffset((offsetRotations)));
}

@Override
public void updateInputs(SwerveAbsoluteEncoderIOInputs inputs) {
// BaseStatusSignal.refreshAll(turnAbsolutePosition);
BaseStatusSignal.refreshAll(turnAbsolutePosition);

inputs.absoluteEncoderOffset = absoluteEncoderOffset;
inputs.inverted = inverted;
Expand All @@ -59,17 +60,17 @@ public void updateInputs(SwerveAbsoluteEncoderIOInputs inputs) {
@Override
public void setInverted(boolean inverted) {
this.inverted = inverted;
// configurator.refresh(magnetSensorConfiguration);
// configurator.apply(magnetSensorConfiguration
// .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
// .withSensorDirection((inverted ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive))
// );
configurator.refresh(magnetSensorConfiguration);
configurator.apply(magnetSensorConfiguration
.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
.withSensorDirection((inverted ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive))
);
}

@Override
public Rotation2d getTurnAbsolutePosition() {
// CHECK: Subtracting offset instead of adding offset
return Rotation2d.fromRotations(CaNcoder.getAbsolutePosition().getValueAsDouble()).plus(Rotation2d.fromDegrees(absoluteEncoderOffset));
return Rotation2d.fromRotations(CaNcoder.getAbsolutePosition().getValueAsDouble());
// return new Rotation2d();
}

Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,7 @@ public SwerveModuleState runState(SwerveModuleState state) {
// }

if(!offsetedTurnEncoder && absoluteEncoder.getTurnAbsolutePosition().getDegrees() != 0) {
offsetedTurnEncoder = turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition().getDegrees()) == StatusCode.OK ? true : false;
offsetedTurnEncoder = turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition()) == StatusCode.OK ? true : false;
}

//feeds value directly to encoder if it is sim.
Expand All @@ -135,6 +135,9 @@ public SwerveModuleState runState(SwerveModuleState state) {
// Prevents the turn motor from doing unneeded rotations
var optimizedState = SwerveModuleState.optimize(state, getAngle());

System.out.println(angleSetPoint + " " + moduleName);
System.out.println(getAngle() + " " + moduleName);

angleSetPoint = optimizedState.angle.getDegrees();
speedSetPoint = (Math.cos(Units.degreesToRadians(getAngle().getDegrees() - angleSetPoint)) * optimizedState.speedMetersPerSecond) / (SwerveDrive.wheelRadius * Math.PI * 2);

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ public default double getVelocity() {
/**
* Sets the motors position
*/
public default StatusCode setEncoderPosition(double positionDeg) {
public default StatusCode setEncoderPosition(Rotation2d positionDeg) {
return StatusCode.OK;
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,8 +111,8 @@ public void setVoltage(double volts) {
}

@Override
public StatusCode setEncoderPosition(double positionDeg) {
encoder.setPosition(positionDeg / 360);
public StatusCode setEncoderPosition(Rotation2d positionDeg) {
encoder.setPosition(positionDeg.getDegrees());
return StatusCode.OK;
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,8 +146,8 @@ public void setPosition(double setpoint) {
}

@Override
public StatusCode setEncoderPosition(double positionDeg) {
encoder.setPosition(positionDeg / 360);
public StatusCode setEncoderPosition(Rotation2d positionDeg) {
encoder.setPosition(positionDeg.getRotations() * gearRatio);
return StatusCode.OK;
}

Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ public SwerveMotorIOTalonFX(int canID, String canbus, double gearRatio) {
driveConfig = new TalonFXConfiguration();
driveConfig.CurrentLimits.SupplyCurrentLimit = CURRENT_LIMIT;
driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
configurator.apply(driveConfig);
//TODO: Apparently this might not work (idk what it's even supposed to do)
configurator.apply(driveConfig.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(VOLTAGE_CLOSED_LOOP_RAMP_PERIOD));
Expand Down Expand Up @@ -145,8 +146,8 @@ public double getVelocity() {
}

@Override
public StatusCode setEncoderPosition(double positionDeg) {
return motor.setPosition(Units.degreesToRotations(positionDeg) * gearRatio);
public StatusCode setEncoderPosition(Rotation2d positionDeg) {
return motor.setPosition(positionDeg.getRotations() * gearRatio);
// motor.setPosition((positionDeg/360) * gearRatio);
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,8 +124,8 @@ public void setVoltage(double volts) {
}

@Override
public StatusCode setEncoderPosition(double positionDeg) {
return dummyTalon.setPosition((positionDeg/360) * gearing);
public StatusCode setEncoderPosition(Rotation2d positionDeg) {
return dummyTalon.setPosition(positionDeg.getRotations() * gearing);
}

@Override
Expand Down

0 comments on commit 7a84cd2

Please sign in to comment.