mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
some aimtocenter stuff, unfinished + has errors
This commit is contained in:
evan
@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.AimToCenter;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
//import frc4388.robot.subsystems.Intake;
|
||||
@@ -37,23 +38,25 @@ public class RobotContainer {
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
private final TalonFX m_testMotor = new TalonFX(2);
|
||||
|
||||
/* Subsystems
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
|
||||
m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
|
||||
m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
|
||||
m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
|
||||
m_robotMap.leftFrontEncoder,
|
||||
m_robotMap.rightFrontEncoder,
|
||||
m_robotMap.leftBackEncoder,
|
||||
m_robotMap.rightBackEncoder
|
||||
);
|
||||
*/
|
||||
/*
|
||||
* Subsystems
|
||||
* private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
* m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
|
||||
* m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
|
||||
* m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
|
||||
* m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
|
||||
* m_robotMap.leftFrontEncoder,
|
||||
* m_robotMap.rightFrontEncoder,
|
||||
* m_robotMap.leftBackEncoder,
|
||||
* m_robotMap.rightBackEncoder
|
||||
* );
|
||||
*/
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
private final Hood m_robotHood = new Hood();
|
||||
private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||
private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_)
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
@@ -78,7 +81,7 @@ public class RobotContainer {
|
||||
//Turret default command
|
||||
|
||||
m_robotTurret.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftXAxis()), m_robotTurret));
|
||||
new AimToCenter(m_robotTurret, m_drive)
|
||||
// m_robotTurret.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotTurret.aimToCenter()));
|
||||
}
|
||||
@@ -101,21 +104,22 @@ public class RobotContainer {
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
// activates "BoomBoom"
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
|
||||
.whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
|
||||
// activates "BoomBoom"
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
|
||||
.whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
|
||||
/* Driver Buttons */
|
||||
//activates intake
|
||||
// activates intake
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON);
|
||||
// .whenPressed() -> m_robot
|
||||
/*operator button*/
|
||||
//activates hood
|
||||
// .whenPressed() -> m_robot
|
||||
/* operator button */
|
||||
// activates hood
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(() -> m_robotHood.runHood(0.5d))
|
||||
.whenReleased(() -> m_robotHood.runHood(0.d));
|
||||
.whenPressed(() -> m_robotHood.runHood(0.5d))
|
||||
.whenReleased(() -> m_robotHood.runHood(0.d));
|
||||
// new JoystickButton(getOperatorJoystick());
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
|
||||
@@ -24,118 +24,167 @@ public class RobotMap {
|
||||
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
//configureSwerveMotorControllers();
|
||||
// configureSwerveMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(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 final WPI_TalonFX leftFrontSteerMotor;
|
||||
// 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);
|
||||
|
||||
// void configureSwerveMotorControllers() {
|
||||
// leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
// rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
// leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
// rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
// leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
// rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
// leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
// rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// leftFrontSteerMotor.configFactoryDefault();
|
||||
// leftFrontWheelMotor.configFactoryDefault();
|
||||
// rightFrontSteerMotor.configFactoryDefault();
|
||||
// rightFrontWheelMotor.configFactoryDefault();
|
||||
// leftBackSteerMotor.configFactoryDefault();
|
||||
// leftBackWheelMotor.configFactoryDefault();
|
||||
// rightBackSteerMotor.configFactoryDefault();
|
||||
// rightBackWheelMotor.configFactoryDefault();
|
||||
// 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);
|
||||
// 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.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);
|
||||
// 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);
|
||||
|
||||
// // 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);
|
||||
//}
|
||||
// // 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);
|
||||
// }
|
||||
|
||||
//Shooter Config
|
||||
/*Boom Boom Subsystem*/
|
||||
// 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);
|
||||
|
||||
|
||||
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
// Create motor CANSparkMax
|
||||
void ConfigureShooterMotorControllers() {
|
||||
|
||||
//LEFT FALCON
|
||||
// LEFT FALCON
|
||||
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, 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,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
|
||||
|
||||
//RIGHT FALCON
|
||||
// RIGHT FALCON
|
||||
shooterFalconRight.setInverted(false);
|
||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconRight.configFactoryDefault();
|
||||
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, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
/*Turret Subsytem*/
|
||||
|
||||
|
||||
shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2));
|
||||
shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4));
|
||||
// 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,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
/* Turret Subsytem */
|
||||
|
||||
shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2));
|
||||
shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4));
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -17,7 +17,7 @@ public class AimToCenter extends CommandBase {
|
||||
double x = 0;
|
||||
double y = 0;
|
||||
double m_targetAngle;
|
||||
|
||||
|
||||
// public static Gains m_aimGains;
|
||||
|
||||
public AimToCenter(Turret turret, SwerveDrive drive) {
|
||||
@@ -36,32 +36,34 @@ public class AimToCenter extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
if (x > 0) {
|
||||
m_targetAngle = 180 + Math.atan(y/x) - m_drive.gyro.getAngle();
|
||||
}
|
||||
m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle();
|
||||
}
|
||||
if (x < 0) {
|
||||
m_targetAngle = 360 + Math.atan(y/x) - m_drive.gyro.getAngle();
|
||||
}
|
||||
m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle();
|
||||
}
|
||||
if (x == 0 && y > 0) {
|
||||
m_targetAngle = 270 - m_drive.gyro.getAngle();
|
||||
m_targetAngle = 270 - m_drive.gyro.getAngle();
|
||||
}
|
||||
if (x == 0 && y < 0) {
|
||||
m_targetAngle = 90 - m_drive.gyro.getAngle();
|
||||
}
|
||||
|
||||
|
||||
m_turret.runshooterRotatePID(m_targetAngle);
|
||||
|
||||
}
|
||||
public boolean noIsDeadzone(){
|
||||
|
||||
public boolean isDeadzone() {
|
||||
if ((-20 < m_targetAngle) && (m_targetAngle < 0)) {
|
||||
return false;
|
||||
return true;
|
||||
} else {
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
|
||||
import com.revrobotics.CANSparkMax.ControlType;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANEncoder;
|
||||
@@ -21,16 +20,14 @@ import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.Gains;
|
||||
import com.revrobotics.CANSparkMax.SoftLimitDirection;
|
||||
|
||||
|
||||
|
||||
|
||||
public class Turret extends SubsystemBase {
|
||||
|
||||
|
||||
/** Creates a new Turret. */
|
||||
public BoomBoom m_boomBoomSubsystem;
|
||||
public SwerveDrive m_sDriveSubsystem;
|
||||
|
||||
public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
|
||||
public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID,
|
||||
// MotorType.kBrushless);
|
||||
public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS;
|
||||
SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit;
|
||||
public Gyro m_turretGyro;
|
||||
@@ -41,16 +38,15 @@ public class Turret extends SubsystemBase {
|
||||
|
||||
SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController();
|
||||
public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder();
|
||||
|
||||
//Variables
|
||||
public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument
|
||||
|
||||
// Variables
|
||||
public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument
|
||||
|
||||
m_boomBoomRotateMotor = boomBoomRotateMotor;
|
||||
m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
|
||||
m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
|
||||
m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake);
|
||||
|
||||
|
||||
|
||||
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
m_boomBoomRightLimit.enableLimitSwitch(true);
|
||||
@@ -58,7 +54,10 @@ public class Turret extends SubsystemBase {
|
||||
|
||||
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
|
||||
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
|
||||
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit
|
||||
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set
|
||||
// second
|
||||
// soft
|
||||
// limit
|
||||
m_boomBoomRotateMotor.setInverted(false);
|
||||
|
||||
m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
|
||||
@@ -69,38 +68,36 @@ public class Turret extends SubsystemBase {
|
||||
m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
|
||||
public void passRequiredSubsystem( BoomBoom subsystem0, SwerveDrive subsystem1){
|
||||
public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) {
|
||||
m_boomBoomSubsystem = subsystem0;
|
||||
m_sDriveSubsystem = subsystem1;
|
||||
}
|
||||
|
||||
public void runTurretWithInput(double input) {
|
||||
m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
|
||||
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
|
||||
}
|
||||
|
||||
public void runshooterRotatePID(double targetAngle) {
|
||||
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
|
||||
m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition);
|
||||
targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
|
||||
m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public void resetGyroShooterRotate()
|
||||
{
|
||||
public void resetGyroShooterRotate() {
|
||||
m_boomBoomRotateEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getboomBoomRotatePosition()
|
||||
{
|
||||
public double getboomBoomRotatePosition() {
|
||||
return m_boomBoomRotateEncoder.getPosition();
|
||||
}
|
||||
|
||||
public double getBoomBoomAngleDegrees() {
|
||||
return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT;
|
||||
return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
|
||||
/ ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user