Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

rotation

Browse Source
This commit is contained in:
Abhishrek05
2024-04-11 17:37:48 -06:00
parent 5d93f9028c
commit 1a56abe474
3 changed files with 36 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -65,7 +65,7 @@ public final class Constants {
public static final class PIDConstants { public static final class PIDConstants {
public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1; 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(3.0, 0.0, 0.0, 0.0, 0, 0.0);
} }
public static final class AutoConstants { public static final class AutoConstants {
src/main/java/frc4388/robot/RobotMap.java
+10 -8
View File
@@ -16,6 +16,7 @@ import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.CANcoder;
@@ -96,23 +97,24 @@ public class RobotMap {
.withMotorOutput(new MotorOutputConfigs() .withMotorOutput(new MotorOutputConfigs()
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
.withNeutralMode(NeutralModeValue.Brake) .withNeutralMode(NeutralModeValue.Brake)
// .withInverted(InvertedValue.Clockwise_Positive)
); );
for (TalonFX motor : new TalonFX[]{leftFrontWheel, leftFrontSteer, rightFrontWheel, rightFrontSteer, leftBackWheel, leftBackSteer, rightBackWheel, rightBackSteer}) { for (TalonFX motor : new TalonFX[]{leftFrontWheel, leftFrontSteer, rightFrontWheel, rightFrontSteer, leftBackWheel, leftBackSteer, rightBackWheel, rightBackSteer}) {
motor.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); motor.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
} }
// config factory default // config factory default
leftFrontWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // leftFrontWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
leftFrontSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // leftFrontSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // rightFrontWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // rightFrontSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // leftBackWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // leftBackSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // rightBackWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS); // rightBackSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
// // config open loop ramp // // config open loop ramp
// leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+25 -14
View File
@@ -27,6 +27,7 @@ import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -57,14 +58,14 @@ public class SwerveModule extends SubsystemBase {
this.selfid = swerveId; this.selfid = swerveId;
swerveId++; swerveId++;
// TalonFXConfiguration angleConfig = new TalonFXConfiguration(); // TalonFXConfiguration angleConfig = new TalonFXConfiguration();
TalonFXConfiguration cfg = new TalonFXConfiguration() // TalonFXConfiguration cfg = new TalonFXConfiguration()
.withSlot0(new Slot0Configs() // .withSlot0(new Slot0Configs()
.withKP(swerveGains.kP) // .withKP(swerveGains.kP)
.withKI(swerveGains.kI) // .withKI(swerveGains.kI)
.withKD(swerveGains.kD)) // .withKD(swerveGains.kD))
.withFeedback(new FeedbackConfigs() // .withFeedback(new FeedbackConfigs()
.withFeedbackRemoteSensorID(encoder.getDeviceID()) // .withFeedbackRemoteSensorID(encoder.getDeviceID())
.withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)); // .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder));
// angleConfig.slot0.kP = swerveGains.kP; // angleConfig.slot0.kP = swerveGains.kP;
// angleConfig.slot0.kI = swerveGains.kI; // angleConfig.slot0.kI = swerveGains.kI;
// angleConfig.slot0.kD = swerveGains.kD; // angleConfig.slot0.kD = swerveGains.kD;
@@ -76,13 +77,23 @@ public class SwerveModule extends SubsystemBase {
// angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); // angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
// angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; // angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
// angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.getConfigurator().apply(cfg); angleMotor.getConfigurator().apply(new Slot0Configs()
.withKP(swerveGains.kP)
.withKI(swerveGains.kI)
.withKD(swerveGains.kD));
angleMotor.getConfigurator().apply(new FeedbackConfigs()
.withFeedbackRemoteSensorID(encoder.getDeviceID())
.withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder));
//.apply(cfg);
// angleMotor.configAllSettings(angleConfig); // angleMotor.configAllSettings(angleConfig);
//encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
CANcoderConfiguration coder_cfg = new CANcoderConfiguration() CANcoderConfiguration coder_cfg = new CANcoderConfiguration()
.withMagnetSensor(new MagnetSensorConfigs() .withMagnetSensor(new MagnetSensorConfigs()
.withMagnetOffset(offset)); .withMagnetOffset(offset)
.withSensorDirection(SensorDirectionValue.Clockwise_Positive));
encoder.getConfigurator().apply(coder_cfg); encoder.getConfigurator().apply(coder_cfg);
reset(0); reset(0);
// encoder.configMagnetOffset(offset); // encoder.configMagnetOffset(offset);
@@ -192,7 +203,7 @@ public class SwerveModule extends SubsystemBase {
// calculate the difference between our current rotational position and our new rotational position // calculate the difference between our current rotational position and our new rotational position
// Rotation2d rotationDelta = state.angle.minus(currentRotation); // Rotation2d rotationDelta = state.angle.minus(currentRotation);
Rotation2d targetAngle = currentRotation.plus(state.angle);
// rotationDelta.getDegrees(); // rotationDelta.getDegrees();
// calculate the new absolute position of the SwerveModule based on the difference in rotation // calculate the new absolute position of the SwerveModule based on the difference in rotation
@@ -206,14 +217,14 @@ public class SwerveModule extends SubsystemBase {
// double currentTicks = encoder.getPosition().getValue() / 0.087890625; // double currentTicks = encoder.getPosition().getValue() / 0.087890625;
// angleMotor.setControl(new PositionVoltage(currentTicks + deltaTicks)); // angleMotor.setControl(new PositionVoltage(currentTicks + deltaTicks));
System.out.println(desiredState.angle.getDegrees()); System.out.println(targetAngle.getDegrees());
angleMotor.setControl(new PositionVoltage(desiredState.angle.getDegrees()/360)); angleMotor.setControl(new PositionVoltage(targetAngle.getDegrees()/360.d));
// angleMotor.setControl(new PositionVoltage(0)); // angleMotor.setControl(new PositionVoltage(0));
// angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
// driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
} }
public void reset(double position) { public void reset(double position) {