mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 00:38:02 -06:00
phoenix 6
This commit is contained in:
C4llSiqn
@@ -11,7 +11,14 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
// CTRE Motor configuration classes
|
||||
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
@@ -46,22 +53,22 @@ public class RobotMap {
|
||||
|
||||
|
||||
/* Swreve Drive Subsystem */
|
||||
public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
|
||||
public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
|
||||
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
|
||||
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
|
||||
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
|
||||
|
||||
|
||||
public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
|
||||
public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
|
||||
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
|
||||
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
|
||||
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
|
||||
public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
|
||||
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
|
||||
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
|
||||
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
|
||||
public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
|
||||
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
|
||||
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
|
||||
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
|
||||
|
||||
/* Shooter Subsystem */
|
||||
public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID);
|
||||
@@ -83,68 +90,79 @@ public class RobotMap {
|
||||
}
|
||||
|
||||
void configureDriveMotorControllers() {
|
||||
TalonFXConfiguration cfg = new TalonFXConfiguration()
|
||||
.withOpenLoopRamps(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE))
|
||||
.withClosedLoopRamps(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE))
|
||||
.withMotorOutput(new MotorOutputConfigs()
|
||||
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
);
|
||||
|
||||
for (TalonFX motor : new TalonFX[]{leftFrontWheel, leftFrontSteer, rightFrontWheel, rightFrontSteer, leftBackWheel, leftBackSteer, rightBackWheel, rightBackSteer}) {
|
||||
motor.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
}
|
||||
// config factory default
|
||||
leftFrontWheel.configFactoryDefault();
|
||||
leftFrontSteer.configFactoryDefault();
|
||||
leftFrontWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configFactoryDefault();
|
||||
rightFrontSteer.configFactoryDefault();
|
||||
rightFrontWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configFactoryDefault();
|
||||
leftBackSteer.configFactoryDefault();
|
||||
leftBackWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configFactoryDefault();
|
||||
rightBackSteer.configFactoryDefault();
|
||||
rightBackWheel.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.getConfigurator().apply(cfg, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config open loop ramp
|
||||
leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// // config open loop ramp
|
||||
// leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config closed loop ramp
|
||||
leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// // config closed loop ramp
|
||||
// leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config neutral deadband
|
||||
leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// // config neutral deadband
|
||||
// leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
// rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// set neutral mode
|
||||
leftFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
// // set neutral mode
|
||||
// leftFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
// rightFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
// leftBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
// rightBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
leftFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
// leftFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
// rightFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
// leftBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
// rightBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
// initialize SwerveModules
|
||||
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
|
||||
|
||||
@@ -14,6 +14,9 @@ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.SensorInitializationStrategy;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
@@ -26,16 +29,16 @@ import frc4388.utility.Gains;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder encoder;
|
||||
private TalonFX driveMotor;
|
||||
private TalonFX angleMotor;
|
||||
private CANcoder encoder;
|
||||
private int selfid;
|
||||
// private ConfigurableDouble offsetGetter;
|
||||
private static int swerveId = 0;
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
|
||||
public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.encoder = encoder;
|
||||
@@ -48,6 +51,9 @@ public class SwerveModule extends SubsystemBase {
|
||||
angleConfig.slot0.kD = swerveGains.kD;
|
||||
|
||||
// use the CANcoder as the remote sensor for the primary TalonFX PID
|
||||
|
||||
new Slot0Configs().
|
||||
|
||||
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
|
||||
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
|
||||
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
@@ -56,7 +62,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
//encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
|
||||
reset(0);
|
||||
encoder.configMagnetOffset(offset);
|
||||
|
||||
driveMotor.
|
||||
driveMotor.setSelectedSensorPosition(0);
|
||||
driveMotor.config_kP(0, 0.2);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user