From e32379867a61eb104544b90d917b7ab48d704e25 Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Mon, 20 Feb 2017 11:00:10 -0800 Subject: [PATCH] fix shooter commands --- .../shooter/ReverseIndexWheelContinuous.java | 1 + .../shooter/RunFlyWheelContinuous.java | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java index 79a0f23..05ab564 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java @@ -44,6 +44,7 @@ public class ReverseIndexWheelContinuous extends Command { // Called once after isFinished returns true @Override protected void end() { + shooter.stopIndexWheel(); } // Called when another command which requires one or more of the same diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java index ff4f659..047fad4 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java @@ -26,6 +26,7 @@ public class RunFlyWheelContinuous extends Command { private double wheelI; private double wheelD; private double target; + double shooterSpeed = 0; public RunFlyWheelContinuous() { this.wheelP = this.shooter.wheelP; @@ -33,10 +34,10 @@ public class RunFlyWheelContinuous extends Command { this.wheelD = this.shooter.wheelD; this.wheelController = new PIDController(this.wheelP, this.wheelI, this.wheelD); - this.wheelController.setDoneRange(0.5); + this.wheelController.setDoneRange(10); this.wheelController.setMaxOutput(1.0); this.wheelController.setMinDoneCycles(3); - this.target = this.shooter.getCurrentShootingSpeed(); + this.target = 2700; } @Override @@ -46,12 +47,14 @@ public class RunFlyWheelContinuous extends Command { @Override protected void execute() { - // double shooterSpeed = this.wheelController - // .calcPID(this.shooter.getShooterRPM()); - // - // this.shooter.setFlyWheelMotorVal(shooterSpeed); - System.out.println(shooter.getShooterRPM()); - this.shooter.setFlyWheelMotorVal(this.shooter.CURRENT_SHOOTING_SPEED); + double calculatedShooterIncrement = this.wheelController + .calcPID(this.shooter.getShooterRPM()); + shooterSpeed += calculatedShooterIncrement; + if (shooterSpeed > 1.0) + this.shooter.setFlyWheelMotorVal(1.0); + else + this.shooter.setFlyWheelMotorVal(shooterSpeed); + // this.shooter.setFlyWheelMotorVal(this.shooter.CURRENT_SHOOTING_SPEED); } @Override -- 2.30.2