Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity
Files
b0697201453a9128b861a13b5422009b3936c744
2022NoWayHome/src/main/java/frc4388/robot/RobotMap.java
T
nathanrsxtn b069720145 Remove angle offset from shooter table 
Fix logging using format strings
Add autonomous chooser to the dashboard
Update path recorder
Move path commands to a class
2022-04-19 20:36:59 -06:00

282 lines
17 KiB
Java
Raw Blame History

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.ClimberConstants;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SerializerConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
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.
*/
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 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
// #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);
// current limits
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
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);
}
void configureExtenderMotors() {
extenderMotor.restoreFactoryDefaults();
extenderMotor.setInverted(true);
extenderMotor.setIdleMode(IdleMode.kBrake);
}
void configureIntakeMotors() {
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);
}
void configureSerializerMotors() {
serializerBelt.restoreFactoryDefaults();
}
void configureStorageMotors() {
storageMotor.restoreFactoryDefaults();
storageMotor.setIdleMode(IdleMode.kCoast);
}
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);
shooterFalconRight.configFactoryDefault();
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
shooterFalconRight.setInverted(false);
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedloopRamp(0.75, 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 Subsystem
shooterTurret.restoreFactoryDefaults();
shooterTurret.setIdleMode(IdleMode.kBrake);
shooterTurret.setInverted(true);
// Hood Subsystem
angleAdjusterMotor.restoreFactoryDefaults();
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
angleAdjusterMotor.setInverted(true);
}
void configureClimberMotors() {
elbow.configFactoryDefault();
elbow.setNeutralMode(NeutralMode.Brake);
}
private void registerDevices() {
SendableRegistry.setName(frontLeftSteerMotor, "Drive", "Left Front Steer Motor");
SendableRegistry.setName(frontLeftDriveMotor, "Drive", "Left Front Drive Motor");
SendableRegistry.setName(frontRightSteerMotor, "Drive", "Right Front Steer Motor");
SendableRegistry.setName(frontRightDriveMotor, "Drive", "Right Front Drive Motor");
SendableRegistry.setName(backLeftSteerMotor, "Drive", "Left Back Steer Motor");
SendableRegistry.setName(backLeftDriveMotor, "Drive", "Left Back Drive Motor");
SendableRegistry.setName(backRightSteerMotor, "Drive", "Right Back Steer Motor");
SendableRegistry.setName(backRightDriveMotor, "Drive", "Right Back Drive Motor");
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");
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");
SendableRegistry.setName(elbow, "Climber", "Elbow");
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
}
}
Reference in New Issue View Git Blame Copy Permalink