mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Small refactoring
This commit is contained in:
nathanrsxtn
@@ -4,7 +4,6 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
@@ -16,7 +15,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
@@ -30,293 +28,255 @@ import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.shuffleboard.SendableCANSparkMax;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
* testing and modularization.
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit testing and
|
||||
* modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
// #region Swerve Subsystem
|
||||
public final WPI_TalonFX frontLeftSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX frontLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX frontRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX frontRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX backLeftSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX backLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX backRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX backRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
|
||||
public final CANCoder frontLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder frontRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder backLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder backRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
public RobotMap() {
|
||||
// configureLEDMotorControllers();
|
||||
configureSwerveMotorControllers();
|
||||
configureShooterMotorControllers();
|
||||
configureIntakeMotors();
|
||||
configureExtenderMotors();
|
||||
configureSerializerMotors();
|
||||
configureStorageMotors();
|
||||
configureClimberMotors();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
// public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {}
|
||||
|
||||
|
||||
/* Swerve Subsystem */
|
||||
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public SwerveModule frontLeft;
|
||||
public SwerveModule frontRight;
|
||||
public SwerveModule backLeft;
|
||||
public SwerveModule backRight;
|
||||
|
||||
public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
|
||||
// #endregion Swerve Subsystem
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule rightBack;
|
||||
// #region Extender Subsystem
|
||||
public final CANSparkMax extenderMotor = new SendableCANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
||||
// #endregion Extender Subsystem
|
||||
|
||||
// #region Intake Subsystem
|
||||
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
|
||||
// #endregion Intake Subsystem
|
||||
|
||||
// #region Serializer Subsystem
|
||||
public final CANSparkMax serializerBelt = new SendableCANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
|
||||
// #endregion Serializer Subsystem
|
||||
|
||||
// #region Storage Subsystem
|
||||
public final CANSparkMax storageMotor = new SendableCANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
|
||||
// #endregion Storage Subsystem
|
||||
|
||||
// #region Shooter System
|
||||
// #region Boom Boom Subsystem
|
||||
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
|
||||
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
|
||||
// #endregion Boom Boom Subsystem
|
||||
|
||||
// #region Turret Subsystem
|
||||
public final CANSparkMax shooterTurret = new SendableCANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
// #endregion Turret Subsystem
|
||||
|
||||
// #region Hood Subsystem
|
||||
public final CANSparkMax angleAdjusterMotor = new SendableCANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
// #endregion Hood Subsystem
|
||||
// #endregion Shooter System
|
||||
|
||||
// #region Climber Subsystem
|
||||
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID);
|
||||
// #endregion Climber Subsystem
|
||||
|
||||
// #region Claws Subsystem
|
||||
public final Servo leftClaw = new Servo(1);
|
||||
public final Servo rightClaw = new Servo(2);
|
||||
// #endregion Claws Subsystem
|
||||
|
||||
// #region LED Subsystem
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
// #endregion LED Subsystem
|
||||
|
||||
public RobotMap() {
|
||||
configureSwerveMotorControllers();
|
||||
configureExtenderMotors();
|
||||
configureIntakeMotors();
|
||||
configureSerializerMotors();
|
||||
configureStorageMotors();
|
||||
configureShooterMotorControllers();
|
||||
configureClimberMotors();
|
||||
registerDevices();
|
||||
}
|
||||
|
||||
void configureSwerveMotorControllers() {
|
||||
frontLeftSteerMotor.configFactoryDefault();
|
||||
frontLeftDriveMotor.configFactoryDefault();
|
||||
frontRightSteerMotor.configFactoryDefault();
|
||||
frontRightDriveMotor.configFactoryDefault();
|
||||
backLeftSteerMotor.configFactoryDefault();
|
||||
backLeftDriveMotor.configFactoryDefault();
|
||||
backRightSteerMotor.configFactoryDefault();
|
||||
backRightDriveMotor.configFactoryDefault();
|
||||
|
||||
frontLeftSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontLeftDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
frontLeftDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontLeftSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
frontLeftDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontLeftSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
frontLeftSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
frontRightSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
backLeftSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
backRightSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
frontLeftDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
frontRightDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
backLeftDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
backRightDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
SendableRegistry.setName(leftFrontSteerMotor, "SwerveDrive", "Left Front Steer Motor");
|
||||
SendableRegistry.setName(leftFrontWheelMotor, "SwerveDrive", "Left Front Wheel Motor");
|
||||
SendableRegistry.setName(rightFrontSteerMotor, "SwerveDrive", "Right Front Steer Motor");
|
||||
SendableRegistry.setName(rightFrontWheelMotor, "SwerveDrive", "Right Front Wheel Motor");
|
||||
SendableRegistry.setName(leftBackSteerMotor, "SwerveDrive", "Left Back Steer Motor");
|
||||
SendableRegistry.setName(leftBackWheelMotor, "SwerveDrive", "Left Back Wheel Motor");
|
||||
SendableRegistry.setName(rightBackSteerMotor, "SwerveDrive", "Right Back Steer Motor");
|
||||
SendableRegistry.setName(rightBackWheelMotor, "SwerveDrive", "Right Back Wheel Motor");
|
||||
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
rightFrontSteerMotor.configFactoryDefault();
|
||||
rightFrontWheelMotor.configFactoryDefault();
|
||||
leftBackSteerMotor.configFactoryDefault();
|
||||
leftBackWheelMotor.configFactoryDefault();
|
||||
rightBackSteerMotor.configFactoryDefault();
|
||||
rightBackWheelMotor.configFactoryDefault();
|
||||
|
||||
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
NeutralMode mode = NeutralMode.Coast;
|
||||
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftFrontWheelMotor.setNeutralMode(mode);// Coast
|
||||
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontWheelMotor.setNeutralMode(mode);// Coast
|
||||
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackWheelMotor.setNeutralMode(mode);// Coast
|
||||
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackWheelMotor.setNeutralMode(mode);// Coast
|
||||
|
||||
// current limits
|
||||
|
||||
leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
|
||||
SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
|
||||
SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder,
|
||||
SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder,
|
||||
SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
frontLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
frontRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
frontLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
frontRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
frontLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
frontRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
frontLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
frontRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
frontLeft = new SwerveModule(frontLeftDriveMotor, frontLeftSteerMotor, frontLeftEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||
frontRight = new SwerveModule(frontRightDriveMotor, frontRightSteerMotor, frontRightEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
backLeft = new SwerveModule(backLeftDriveMotor, backLeftSteerMotor, backLeftEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
backRight = new SwerveModule(backRightDriveMotor, backRightSteerMotor, backRightEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
frontLeftSteerMotor.configRemoteFeedbackFilter(frontLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configRemoteFeedbackFilter(backLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configRemoteFeedbackFilter(frontRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configRemoteFeedbackFilter(backRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Climb Subsystem */
|
||||
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
|
||||
void configureExtenderMotors() {
|
||||
extenderMotor.restoreFactoryDefaults();
|
||||
extenderMotor.setInverted(true);
|
||||
extenderMotor.setIdleMode(IdleMode.kBrake);
|
||||
}
|
||||
|
||||
private void configureClimberMotors() {
|
||||
SendableRegistry.setName(elbow, "Climber", "Elbow");
|
||||
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
|
||||
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
|
||||
elbow.configFactoryDefault();
|
||||
elbow.setNeutralMode(NeutralMode.Brake);
|
||||
}
|
||||
/* Hooks Subsystem */
|
||||
public final Servo leftClaw = new Servo(1);
|
||||
public final Servo rightClaw = new Servo(2);
|
||||
void configureIntakeMotors() {
|
||||
intakeMotor.configFactoryDefault();
|
||||
intakeMotor.setInverted(false);
|
||||
intakeMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
// Shooter Config
|
||||
/* Boom Boom Subsystem */
|
||||
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
|
||||
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
|
||||
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
}
|
||||
|
||||
// turret subsystem
|
||||
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
void configureSerializerMotors() {
|
||||
serializerBelt.restoreFactoryDefaults();
|
||||
}
|
||||
|
||||
// hood subsystem
|
||||
public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
void configureStorageMotors() {
|
||||
storageMotor.restoreFactoryDefaults();
|
||||
storageMotor.setIdleMode(IdleMode.kCoast);
|
||||
}
|
||||
|
||||
// Create motor CANSparkMax
|
||||
void configureShooterMotorControllers() {
|
||||
|
||||
// LEFT FALCON
|
||||
SendableRegistry.setName(shooterFalconLeft, "BoomBoom", "Left Motor");
|
||||
void configureShooterMotorControllers() {
|
||||
// Boom Boom Subsystem
|
||||
shooterFalconLeft.configFactoryDefault();
|
||||
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconLeft.setInverted(true);
|
||||
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
// RIGHT FALCON
|
||||
SendableRegistry.setName(shooterFalconRight, "BoomBoom", "Right Motor");
|
||||
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
shooterFalconRight.configFactoryDefault();
|
||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconRight.setInverted(false);
|
||||
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
// m_shooterFalconRight.configPeakOutputForward(0,
|
||||
// ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
|
||||
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
// shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); //(comment it in if necessary)
|
||||
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
shooterFalconRight.follow(shooterFalconLeft);
|
||||
|
||||
// turret
|
||||
|
||||
// Turret Subsystem
|
||||
shooterTurret.restoreFactoryDefaults();
|
||||
shooterTurret.setIdleMode(IdleMode.kBrake);
|
||||
shooterTurret.setInverted(true);
|
||||
|
||||
// hood subsystem
|
||||
|
||||
// Hood Subsystem
|
||||
angleAdjusterMotor.restoreFactoryDefaults();
|
||||
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
|
||||
angleAdjusterMotor.setInverted(true);
|
||||
}
|
||||
|
||||
/* Serializer Subsystem */
|
||||
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
|
||||
public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
|
||||
void configureClimberMotors() {
|
||||
elbow.configFactoryDefault();
|
||||
elbow.setNeutralMode(NeutralMode.Brake);
|
||||
}
|
||||
|
||||
/* Intake Subsystem */
|
||||
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
|
||||
public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
||||
private void registerDevices() {
|
||||
SendableRegistry.setName(frontLeftSteerMotor, "SwerveDrive", "Left Front Steer Motor");
|
||||
SendableRegistry.setName(frontLeftDriveMotor, "SwerveDrive", "Left Front Drive Motor");
|
||||
SendableRegistry.setName(frontRightSteerMotor, "SwerveDrive", "Right Front Steer Motor");
|
||||
SendableRegistry.setName(frontRightDriveMotor, "SwerveDrive", "Right Front Drive Motor");
|
||||
SendableRegistry.setName(backLeftSteerMotor, "SwerveDrive", "Left Back Steer Motor");
|
||||
SendableRegistry.setName(backLeftDriveMotor, "SwerveDrive", "Left Back Drive Motor");
|
||||
SendableRegistry.setName(backRightSteerMotor, "SwerveDrive", "Right Back Steer Motor");
|
||||
SendableRegistry.setName(backRightDriveMotor, "SwerveDrive", "Right Back Drive Motor");
|
||||
|
||||
void configureIntakeMotors() {
|
||||
SendableRegistry.setName(intakeMotor, "Intake", "Intake Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) extenderMotor, "Intake", "Extender Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) serializerBelt, "Intake", "Serializer Belt");
|
||||
SendableRegistry.setName((SendableCANSparkMax) storageMotor, "Intake", "Storage Motor");
|
||||
|
||||
intakeMotor.configFactoryDefault();
|
||||
intakeMotor.setInverted(false);
|
||||
intakeMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
}
|
||||
SendableRegistry.setName(shooterFalconLeft, "Shooter", "BoomBoom Left Motor");
|
||||
SendableRegistry.setName(shooterFalconRight, "Shooter", "BoomBoom Right Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) shooterTurret, "Shooter", "Turret");
|
||||
SendableRegistry.setName((SendableCANSparkMax) angleAdjusterMotor, "Shooter", "Hood");
|
||||
|
||||
void configureExtenderMotors() {
|
||||
SendableRegistry.add(new SendableCANSparkMax(extenderMotor), "Intake", "Extender Motor");
|
||||
|
||||
extenderMotor.restoreFactoryDefaults();
|
||||
extenderMotor.setInverted(true);
|
||||
extenderMotor.setIdleMode(IdleMode.kBrake);
|
||||
}
|
||||
|
||||
void configureSerializerMotors() {
|
||||
SendableRegistry.add(new SendableCANSparkMax(serializerBelt), "Intake", "Serializer Belt");
|
||||
SendableRegistry.setName(serializerBeam, "Intake", "Serializer Beam");
|
||||
|
||||
serializerBelt.restoreFactoryDefaults();
|
||||
}
|
||||
|
||||
/* Storage Subsystem */
|
||||
public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
void configureStorageMotors() {
|
||||
SendableRegistry.add(new SendableCANSparkMax(storageMotor), "Intake", "Storage Motor");
|
||||
storageMotor.restoreFactoryDefaults();
|
||||
storageMotor.setIdleMode(IdleMode.kCoast);
|
||||
SendableRegistry.setName(elbow, "Climber", "Elbow");
|
||||
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
|
||||
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user