From 44dd9f3a4e290c4d2b9b7d3411db361a7fd0ba7b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 20 Jun 2024 11:29:41 -0600 Subject: [PATCH] Pointing to angle --- src/main/java/frc4388/robot/Constants.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../robot/subsystems/SwerveModule.java | 67 ++++++------------- 3 files changed, 21 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5aebb9f..296c8d4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,7 +67,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + public static final Gains SWERVE_GAINS = new Gains(32, 0.0, 0.0, 0.0, 0, 0.0); public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index adf7d5e..a48a906 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -55,7 +55,7 @@ public class SwerveDrive extends SubsystemBase { public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ double ang = (Math.atan2(rightStick.getY(), rightStick.getX()) / (Math.PI*2)); - System.out.println(ang); + // System.out.println(ang); module.go(ang); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 8d5322f..32fe785 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -16,6 +16,7 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; // import com.ctre.phoenix.sensors.SensorInitializationStrategy; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -25,6 +26,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.geometry.Translation2d; @@ -56,60 +58,28 @@ public class SwerveModule extends SubsystemBase { this.angleMotor = angleMotor; this.encoder = encoder; - // TalonFXConfiguration pidConfigs = new TalonFXConfiguration(); - // pidConfigs.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output - // pidConfigs.Slot0.kI = 0; // no output for integrated error - // pidConfigs.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output - - // angleMotor.getConfigurator().apply(slot0Configs); - - // var fx_cfg = new FeedbackConfigs(); - // fx_cfg.FeedbackRemoteSensorID = encoder.getDeviceID(); - // fx_cfg.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - // angleMotor.getConfigurator().apply(fx_cfg); - // // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); - // this.selfid = swerveId; - // swerveId++; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - // angleConfig.Slot0.kP = swerveGains.kP; - // angleConfig.Slot0.kI = swerveGains.kI; - // angleConfig.Slot0.kD = swerveGains.kD; - angleConfig.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output - angleConfig.Slot0.kI = 0; // no output for integrated error - angleConfig.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output + angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + angleConfig.Slot0.kP = swerveGains.kP; + angleConfig.Slot0.kI = swerveGains.kI; + angleConfig.Slot0.kD = swerveGains.kD; - // var fx_cfg = new FeedbackConfigs(); angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - // angleConfig.Feedback. = FeedbackDevice.RemoteSensor0; - // angleConfig.Feedback = fx_cfg; - angleMotor.getConfigurator().apply(angleConfig); - // use the CANcoder as the remote sensor for the primary TalonFX PID - // angleConfig.Fee = encoder.getDeviceID(); - // angleConfig.FeedbackSensorSource = RemoteSensorSource.CANCoder; - // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - // angleMotor.getConfigurator().apply(angleConfig); - - // //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); - // reset(0); - // encoder.configMagnetOffset(offset); - - // driveMotor.setSelectedSensorPosition(0); - // driveMotor.config_kP(0, 0.2); + CANcoderConfiguration canconfig = new CANcoderConfiguration(); + canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + encoder.getConfigurator().apply(canconfig); + + rotateToAngle(0); } public void go(double ang){ - double curang = this.encoder.getAbsolutePosition().getValue(); - System.out.println(ang-curang); - - final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); - - // set position to 10 rotations - angleMotor.setControl(m_request.withPosition(ang)); - - // System.out.println(this.cc_pos.getValue()); + // double curang = this.encoder.getAbsolutePosition().getValue(); + System.out.println(getAngle().getDegrees()); + rotateToAngle(ang); } @Override @@ -152,7 +122,7 @@ public class SwerveModule extends SubsystemBase { public Rotation2d getAngle() { // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - return new Rotation2d(); + return Rotation2d.fromRotations(encoder.getAbsolutePosition().getValue()); } public double getAngularVel() { @@ -176,7 +146,8 @@ public class SwerveModule extends SubsystemBase { } public void rotateToAngle(double angle) { - // angleMotor.set(TalonFXControlMode.Position, angle); + final PositionVoltage m_request = new PositionVoltage(angle); + angleMotor.setControl(m_request); } /** @@ -203,7 +174,7 @@ public class SwerveModule extends SubsystemBase { * @param desiredState a SwerveModuleState representing the desired new state of the module // */ public void setDesiredState(SwerveModuleState desiredState) { - // Rotation2d currentRotation = this.getAngle(); + Rotation2d currentRotation = this.getAngle(); // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);