mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 08:48:01 -06:00
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.
This commit is contained in:
Kyra
@@ -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;
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user