From 7f96cecc4d95c2dbae2873f4b6b034c37fffcdb9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 21 Jan 2020 16:45:29 -0700 Subject: [PATCH] Spin up before engaging shooter pid --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/ShooterVelocityControlPID.java | 6 +++++- src/main/java/frc4388/robot/subsystems/Shooter.java | 5 +++-- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9dbda6a..835dff1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,7 +46,7 @@ public final class Constants { public static final int ELEVATOR_SLOT_IDX = 0; public static final int ELEVATOR_PID_LOOP_IDX = 1; public static final int ELEVATOR_TIMEOUT_MS = 30; - public static final Gains ELEVATOR_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } @@ -58,7 +58,7 @@ 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 SHOOTER_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9c45f3e..99b7d52 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,7 +101,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(2300), m_robotShooter)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2300)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 520e501..4c7b058 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,7 +31,11 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_targetVel); + if (m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000){ + m_shooter.runDrumShooter(0.5); + } else { + m_shooter.runDrumShooterVelocityPID(m_targetVel); + } } // 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 24b7461..9452170 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -43,13 +43,14 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - SmartDashboard.putNumber("Shooter Velocity", m_targetVel); + SmartDashboard.putNumber("Shooter Velocity Target", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run - m_targetVel = SmartDashboard.getNumber("Shooter Velocity", m_targetVel); + m_targetVel = SmartDashboard.getNumber("Shooter Velocity Target", m_targetVel); + SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } /**