From 449a3dc8c45c06b69d224b278fa9dc2124bdd668 Mon Sep 17 00:00:00 2001 From: evan Date: Thu, 24 Feb 2022 18:12:57 -0700 Subject: [PATCH] some aimtocenter stuff, unfinished + has errors --- .../java/frc4388/robot/RobotContainer.java | 50 +++-- src/main/java/frc4388/robot/RobotMap.java | 205 +++++++++++------- .../frc4388/robot/commands/AimToCenter.java | 26 ++- .../java/frc4388/robot/subsystems/Turret.java | 41 ++-- 4 files changed, 187 insertions(+), 135 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4fc150f..e8cdbdf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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. * diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1e2b648..7aedfce 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -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)); } - } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 592e008..d463f07 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -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 diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 530de78..4d7ac99 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -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; } } \ No newline at end of file