diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 481a7f5..f81591d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -90,7 +90,8 @@ public final class Constants { 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 DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final double SHOOTER_TURRET_MIN = -1.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8e0dad0..5339635 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -48,7 +48,9 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; +import frc4388.robot.commands.ShootFireGroup; import frc4388.robot.commands.ShootFullGroup; +import frc4388.robot.commands.ShootPrepGroup; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -104,10 +106,11 @@ public class RobotContainer { // 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_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); + m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + // moves the drum not + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); } @@ -137,8 +140,10 @@ public class RobotContainer { // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false); - + .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity()))); + //.whenReleased(new InstantCommand(() -> m_robotShooterAim.runShooterWithInput(0), m_robotShooterAim)); + // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); @@ -160,6 +165,12 @@ public class RobotContainer { //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) .whileHeld(new StorageOutake(m_robotStorage)); + + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); + + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity()))); } /** diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 060334d..855fc48 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -25,7 +25,7 @@ public class ShootFireGroup extends ParallelRaceGroup { */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), + new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), new HoldTarget(m_shooter, m_shooterAim) //new StorageRun(m_storage) diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index 169640e..5d60abb 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java @@ -26,7 +26,7 @@ public class ShootPrepGroup extends ParallelCommandGroup { public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { addCommands( new TrackTarget(m_shooter, m_shooterAim), - new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()) + new ShooterVelocityControlPID(m_shooter) //new StoragePrepAim(m_storage) ); } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index ad295e3..a2a1b77 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -19,23 +19,22 @@ public class ShooterVelocityControlPID extends CommandBase { * @param subsystem The Shooter subsytem * @param targetVel The target velocity */ - public ShooterVelocityControlPID(Shooter subsystem, double targetVel) { + public ShooterVelocityControlPID(Shooter subsystem) { m_shooter = subsystem; - m_targetVel = targetVel; addRequirements(m_shooter); } // Called when the command is initially scheduled. @Override public void initialize() { + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); - m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 69d0721..3de7f36 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -55,6 +55,7 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(false); m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); + m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -101,6 +102,7 @@ public class Shooter extends SubsystemBase { /* Angle Adjustment PID Control */ public void runAngleAdjustPID(double targetAngle) { + targetAngle = addFireAngle(); // Set PID Coefficients m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); @@ -121,17 +123,14 @@ public class Shooter extends SubsystemBase { * @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 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/3); + if (actualVel < targetVel - 1000){ + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up } else{ //PID Based on targetVel - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel/3); //Init PID + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } + m_shooterFalcon.feed(); } public void resetGyroAngleAdj(){