diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2840cdb..c98b94f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -69,6 +69,18 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 9; public static final int EXTENDER_SPARK_ID = 10; } + + 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.4, 0.0005, 13, 0.05, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; + } public static final class ClimberConstants { public static final int CLIMBER_SPARK_ID = 10; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8ecddc4..285d467 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,6 +20,11 @@ import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.commands.ShooterVelocityControlPID; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.subsystems.Drive; @@ -43,6 +48,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Shooter m_robotShooter = new Shooter(); private final Climber m_robotClimber = new Climber(); private final Leveler m_robotLeveler = new Leveler(); private final Storage m_robotStorage = new Storage(); @@ -66,6 +72,8 @@ public class RobotContainer { m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // runs the drum shooter in idle mode + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // runs storage motor at 50 percent @@ -80,6 +88,9 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ + // test command to spin the robot while pressing A on the driver controller + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive); //new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) // .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); @@ -88,15 +99,18 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); - /* PID Test Command */ // runs velocity PID while driving straight new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); @@ -108,9 +122,7 @@ public class RobotContainer { // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); - - //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); + // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java similarity index 51% rename from src/main/java/frc4388/robot/commands/DriveToDistanceMM.java rename to src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 625e522..67b5cb1 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -8,39 +8,31 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Shooter; -public class DriveToDistanceMM extends CommandBase { - Drive m_drive; - double m_distance; - double m_leftTarget; - double m_rightTarget; - +public class ShooterVelocityControlPID extends CommandBase { + Shooter m_shooter; + double m_targetVel; /** - * Creates a new DriveToDistancePID. - * @param subsystem drive subsystem - * @param distance distance to travel in inches + * Creates a new ShooterVelocityControlPID. */ - public DriveToDistanceMM(Drive subsystem, double distance) { + public ShooterVelocityControlPID(Shooter subsystem, double targetVel) { // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_distance = distance * DriveConstants.TICKS_PER_INCH; - addRequirements(m_drive); + m_shooter = subsystem; + m_targetVel = targetVel; + addRequirements(m_shooter); } // Called when the command is initially scheduled. @Override public void initialize() { - m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - //m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); + System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); } // Called once the command ends or is interrupted. @@ -51,10 +43,6 @@ public class DriveToDistanceMM extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ - return true; - } else { - return false; - } + return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 07ba57a..35db61c 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -83,7 +83,6 @@ public class Drive extends SubsystemBase { m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); @@ -239,6 +238,7 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); 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..169e36f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* 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.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.ShooterConstants; + +public class Shooter extends SubsystemBase { + + public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + + double velP; + /** + * Creates a new Shooter subsystem. + */ + public Shooter() { + m_shooterFalcon.configFactoryDefault(); + + m_shooterFalcon.setNeutralMode(NeutralMode.Coast); + + m_shooterFalcon.setInverted(true); + + setShooterGains(); + + m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + int closedLoopTimeMs = 1; + m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); + } + + /** + * Runs drum shooter motor. + * @param speed Speed to set the motor at + */ + 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.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } + /** + * Runs drum shooter velocity PID. + * @param falcon Motor to use + * @param targetVel Target velocity to run motor at + */ + public void runDrumShooterVelocityPID(double targetVel, double actualVel) { + velP = actualVel/targetVel; //Percent of target + double runSpeed = actualVel + (12000*velP); //Ramp up equation + //if (runSpeed > targetVel) {runSpeed = targetVel;} + SmartDashboard.putNumber("shoot", actualVel); + runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ //PID Based on ramp up amount + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); + } + else{ //PID Based on targetVel + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID + } + } +} \ No newline at end of file