From d9bd213ff29377c5b2dc21a2b4190e0812758e21 Mon Sep 17 00:00:00 2001 From: Kyra <101209@psdschools.org> Date: Sat, 18 Jan 2020 15:28:48 -0800 Subject: [PATCH] Create Shooter Subsystem and PID stuff Created shooter subsystem, set up command for shooter to run, set up PID constants, set up gains method in shooter. --- src/main/java/frc4388/robot/Constants.java | 12 ++++ .../java/frc4388/robot/RobotContainer.java | 11 +++- .../java/frc4388/robot/subsystems/Drive.java | 26 ++++----- .../frc4388/robot/subsystems/Elevator.java | 20 ++++--- .../frc4388/robot/subsystems/Shooter.java | 57 +++++++++++++++++++ 5 files changed, 102 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Shooter.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2714b3b..9dbda6a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,6 +51,18 @@ public final class Constants { public static final double ENCODER_TICKS_PER_REV = 2048; } + public static final class ShooterConstants { + public static final int SHOOTER_FALCON_ID = 8; + + /* PID Constants Shooter */ + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final int SHOOTER_TIMEOUT_MS = 30; + public static final Gains SHOOTER_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; + } + public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 25f9528..abdfb35 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Shooter; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -41,6 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); private final Elevator m_robotElevator = new Elevator(); + private final Shooter m_robotShooter = new Shooter(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -57,12 +59,15 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.driveWithInput( getDriverController().getLeftYAxis(), getDriverController().getRightXAxis()), m_robotDrive)); - // drives motor with input from triggers on the opperator controller + + // drives motor with input from triggers on the operator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // moves elevator with one-axis input from the driver controller m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( getOperatorController().getLeftYAxis()), m_robotElevator )); + // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } @@ -85,7 +90,6 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); @@ -100,6 +104,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive)); + + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 833f132..df272f9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,7 +34,7 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - public static Gains m_gains = DriveConstants.DRIVE_GAINS; + public static Gains m_driveGains = DriveConstants.DRIVE_GAINS; /** * Add your docs here. @@ -121,10 +121,10 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - m_gains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - m_gains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - m_gains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - m_gains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); setDriveTrainGains(); @@ -151,16 +151,16 @@ public class Drive extends SubsystemBase { */ public void setDriveTrainGains(){ m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4423466..eec7909 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -21,7 +21,7 @@ public class Elevator extends SubsystemBase { public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); - public static Gains m_gains = ElevatorConstants.ELEVATOR_GAINS; + public static Gains m_elevatorGains = ElevatorConstants.ELEVATOR_GAINS; /** * Creates a new Elevator. */ @@ -41,6 +41,8 @@ public class Elevator extends SubsystemBase { m_talon1.setInverted(InvertType.FollowMaster); m_talon2.setInverted(InvertType.FollowMaster); + setElevatorGains(); + m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); @@ -59,16 +61,16 @@ public class Elevator extends SubsystemBase { } public void setElevatorGains(){ m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); } public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { talon.set(ControlMode.Position, targetPos); diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java new file mode 100644 index 0000000..eb1b6d3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; +import frc4388.robot.Constants.ShooterConstants; + +public class Shooter extends SubsystemBase { + + WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + + /** + * Creates a new Shooter. + */ + public Shooter() { + m_shooterFalcon.configFactoryDefault(); + + m_shooterFalcon.setNeutralMode(NeutralMode.Coast); + + m_shooterFalcon.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs drum shooter motor. + * @param speed + */ + public void runDrumShooter(double speed) { + m_shooterFalcon.set(speed); + } + + /** + * Configures gains for shooter PID. + */ + public void setShooterGains() { + m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } +}