diff --git a/src/main/Modules/KrakenSteerModule.java b/src/main/Modules/KrakenSteerModule.java new file mode 100644 index 0000000..4a9445a --- /dev/null +++ b/src/main/Modules/KrakenSteerModule.java @@ -0,0 +1,102 @@ +package frc.robot; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; + +public class KrakenSteerModule { + TalonFX steerMotor; + CANSparkMax driveMotor; + + + final double WHEEL_DIAMETER = 0.10033; // Metres + final double DRIVE_REDUCTION = (15.0 / 32.0) * (10.0 / 60.0); + final double STEER_REDUCTION = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0); + final double DRIVE_CONVERSION_FACTOR = Math.PI * WHEEL_DIAMETER * DRIVE_REDUCTION; + + double desiredAngle; + + public SwerveModule(ShuffleboardLayout tab, int driveID, int steerID, double offset){ + driveMotor = new CANSparkMax(driveID, MotorType.kBrushless); + steerMotor = new TalonFX(steerCANID); + + steerMotor.configFactoryDefault(); + steerMotor.configIntegratedSensorOffset(offset, 250); + steerMotor.configIntegratedSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition, 250); + steerMotor.configIntegratedSensorAbsoluteRange(AbsoluteSensorRange.Unsigned_0_to_360, 250); + // steerMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 250, 250); // TODO: find out what the different StatusFrameEnhanced Options do and which would be best for a swerve module and a good timeout + steerMotor.setNeutralMode(NeutralMode.Brake); + steerMotor.setSensorPhase(true); // TODO: Check this + //TODO: Check if this needs to be inverted + // steerEncoder.configSensorDirection(false); + + steerMotor.setSmartCurrentLimit(20); + driveMotor.setSmartCurrentLimit(40); + + steerMotor.setIdleMode(IdleMode.kBrake); + driveMotor.setIdleMode(IdleMode.kCoast); + + driveMotor.getEncoder().setPositionConversionFactor(DRIVE_CONVERSION_FACTOR); + driveMotor.getEncoder().setVelocityConversionFactor(DRIVE_CONVERSION_FACTOR / 60.0); + + // steerMotor.getEncoder().setPositionConversionFactor(2 * Math.PI * STEER_REDUCTION); + // steerMotor.getEncoder().setVelocityConversionFactor(2 * Math.PI * STEER_REDUCTION / 60); + // steerMotor.getEncoder().setPosition(steerEncoder.getAbsolutePosition()); + + driveMotor.setInverted(true); + steerMotor.setInverted(false); + + driveMotor.enableVoltageCompensation(12); + + // steerMotor.getPIDController().setP(0.1); + // steerMotor.getPIDController().setI(0.0); + // steerMotor.getPIDController().setD(1.0); + + steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 100); + steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); + steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 20); + + tab.addDouble("Absolute Angle", steerEncoder::getAbsolutePosition); + tab.addDouble("Current Angle", () -> Math.toDegrees(steerMotor.getEncoder().getPosition())); + tab.addDouble("Target Angle", () -> desiredAngle); + tab.addDouble("Velocity", steerMotor.getEncoder()::getVelocity); + } + + public void resetDrivePosition() { + driveMotor.getEncoder().setPosition(0); + } + + public void resetSteerPosition(){ + steerMotor.getEncoder().setPosition(Math.toRadians(steerEncoder.getAbsolutePosition())); + } + + public double getDrivePosition(){ + return driveMotor.getEncoder().getPosition(); + } + + public double getSteerAngle(){ + return steerMotor.getEncoder().getPosition(); // May Switch to absolute Encoder + } + + public void set(double driveVolts, double targetAngle){ + resetSteerPosition(); + + // Put in range of [0, 360) + targetAngle %= 360; + targetAngle += (targetAngle < 0.0) ? 360 : 0; + + desiredAngle = targetAngle; + + double diff = targetAngle - steerEncoder.getAbsolutePosition(); + + if (diff > 90 || diff < -90){ // move to a closer angle and drive backwards + targetAngle = (targetAngle + 180) % 360; + driveVolts *= -1.0; + } + + driveMotor.setVoltage(driveVolts); + // steerMotor.getPIDController().setReference(Math.toRadians(targetAngle), ControlType.kPosition); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/Modules/SwerveModule.java similarity index 100% rename from src/main/java/frc/robot/SwerveModule.java rename to src/main/Modules/SwerveModule.java diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d0c2cdc..72b7908 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; - import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Paths;