diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SL_SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SL_SwerveModule.java index 87fbdb5..310da93 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SL_SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SL_SwerveModule.java @@ -13,7 +13,6 @@ import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.streams.angles.filters.ARateLimit; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.constants.Settings.Swerve.Drive; @@ -91,7 +90,7 @@ public SL_SwerveModule(String id, Translation2d location, int turnCANId, Rotatio Motors.disableStatusFrames(driveMotor, 3, 4, 5, 6); driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD) - .setOutputFilter(x -> true ? 0 : x) + .setOutputFilter(x -> true ? 0 : x) //XXX: This doesnt work .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity()); targetState = new SwerveModuleState(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SimModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SimModule.java index 35d1400..415f8e1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SimModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/evenMoreSwerve/SimModule.java @@ -83,7 +83,7 @@ public SimModule(String id, Translation2d location) { driveSim = new LinearSystemSim<>(identifyVelocityPositionSystem(Drive.kV, Drive.kA)); driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD) - .setOutputFilter(x -> true ? 0 : x) + .setOutputFilter(x -> true ? 0 : x) //Same issue as SL_SwerveModule .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity()); targetState = new SwerveModuleState(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index be4b81b..36e281c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -13,6 +13,7 @@ public class TurretImpl extends Turret { public TurretImpl(int port) { motor = new CANSparkMax(port, MotorType.kBrushless); encoder = motor.getAbsoluteEncoder(Type.kDutyCycle); + encoder.setPositionConversionFactor(-1); //TODO: figure out the conversion factor } @Override @@ -25,9 +26,12 @@ public void setTurretVoltage(double voltage) { motor.setVoltage(voltage); } + public boolean isStalling() { + return motor.getOutputCurrent() > 40; + } + @Override public void periodic2() { - } }