From cf77d84e212b20171a5a3397b26fe7f98f4ba460 Mon Sep 17 00:00:00 2001 From: Christopher Zhu Date: Sat, 4 Feb 2017 13:38:56 -0800 Subject: [PATCH] modify RunFlyWheel and RunFlyWheelContinuous commands --- .../robot/commands/shooter/RunFlyWheel.java | 58 ++++++++------- .../shooter/RunFlyWheelContinuous.java | 47 +++++++----- .../commands/shooter/StartUpFlyWheel.java | 72 ------------------- .../team3501/robot/subsystems/Shooter.java | 6 +- 4 files changed, 66 insertions(+), 117 deletions(-) delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/shooter/StartUpFlyWheel.java diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheel.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheel.java index 6ac5d69..d68b13e 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheel.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheel.java @@ -1,7 +1,9 @@ + package org.usfirst.frc.team3501.robot.commands.shooter; import org.usfirst.frc.team3501.robot.Robot; import org.usfirst.frc.team3501.robot.subsystems.Shooter; +import org.usfirst.frc.team3501.robot.utils.PIDController; import edu.wpi.first.wpilibj.command.Command; @@ -9,52 +11,58 @@ import edu.wpi.first.wpilibj.command.Command; * This command runs the fly wheel at a given speed for a given time. The fly * wheel is intended to shoot balls fed by the intake wheel. * - * @author Shaina + * @author Shaina & Chris */ public class RunFlyWheel extends Command { private Shooter shooter = Robot.getShooter(); - private double time; - - /** - * See JavaDoc comment in class for details - * - * @param motorVal - * value range from -1 to 1 - * @param time - * in seconds, amount of time to run fly wheel motor - */ - public RunFlyWheel(double time) { + private double maxTimeOut; + + private PIDController wheelController; + private double wheelP; + private double wheelI; + private double wheelD; + private double target; + + public RunFlyWheel(double maxTimeOut) { requires(shooter); - this.time = time; + + this.wheelP = this.shooter.wheelP; + this.wheelI = this.shooter.wheelI; + this.wheelD = this.shooter.wheelD; + this.wheelController = new PIDController(this.wheelP, this.wheelI, + this.wheelD); + this.wheelController.setDoneRange(0.5); + this.wheelController.setMaxOutput(1.0); + this.wheelController.setMinDoneCycles(3); + this.target = this.shooter.CURRENT_SHOOTING_SPEED; } // Called just before this Command runs the first time - @Override protected void initialize() { + this.wheelController.setSetPoint(this.target); } // Called repeatedly when this Command is scheduled to run - @Override protected void execute() { - shooter.setFlyWheelMotorVal(shooter.CURRENT_SHOOTING_SPEED); + double shooterSpeed = this.wheelController + .calcPID(this.shooter.getShooterSpeed()); + + this.shooter.setFlyWheelMotorVal(shooterSpeed); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return timeSinceInitialized() >= maxTimeOut; } // Called once after isFinished returns true - @Override protected void end() { - shooter.stopFlyWheel(); + this.shooter.stopFlyWheel(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run - @Override protected void interrupted() { end(); } - - @Override - protected boolean isFinished() { - return timeSinceInitialized() >= time; - } - } 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 1f1d5cd..5e65b3d 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java @@ -2,6 +2,7 @@ package org.usfirst.frc.team3501.robot.commands.shooter; import org.usfirst.frc.team3501.robot.Robot; import org.usfirst.frc.team3501.robot.subsystems.Shooter; +import org.usfirst.frc.team3501.robot.utils.PIDController; import edu.wpi.first.wpilibj.command.Command; @@ -20,42 +21,54 @@ import edu.wpi.first.wpilibj.command.Command; public class RunFlyWheelContinuous extends Command { private Shooter shooter = Robot.getShooter(); - /** - * See JavaDoc comment in class for details - * - * @param motorVal - * value range from -1 to 1 - */ + private PIDController wheelController; + private double wheelP; + private double wheelI; + private double wheelD; + private double target; + public RunFlyWheelContinuous() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); requires(shooter); + + this.wheelP = this.shooter.wheelP; + this.wheelI = this.shooter.wheelI; + this.wheelD = this.shooter.wheelD; + this.wheelController = new PIDController(this.wheelP, this.wheelI, + this.wheelD); + this.wheelController.setDoneRange(0.5); + this.wheelController.setMaxOutput(1.0); + this.wheelController.setMinDoneCycles(3); + this.target = this.shooter.CURRENT_SHOOTING_SPEED; } // Called just before this Command runs the first time - @Override protected void initialize() { + this.wheelController.setSetPoint(this.target); } // Called repeatedly when this Command is scheduled to run - @Override protected void execute() { - shooter.setFlyWheelMotorVal(shooter.CURRENT_SHOOTING_SPEED); + double shooterSpeed = this.wheelController + .calcPID(this.shooter.getShooterSpeed()); + + this.shooter.setFlyWheelMotorVal(shooterSpeed); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; } // Called once after isFinished returns true - @Override protected void end() { + this.shooter.stopFlyWheel(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run - @Override protected void interrupted() { end(); } - - @Override - protected boolean isFinished() { - return false; - } - } diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/StartUpFlyWheel.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/StartUpFlyWheel.java deleted file mode 100644 index 414641b..0000000 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/StartUpFlyWheel.java +++ /dev/null @@ -1,72 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.shooter; - -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.Shooter; -import org.usfirst.frc.team3501.robot.utils.PIDController; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class StartUpFlyWheel extends Command { - private Shooter shooter = Robot.getShooter(); - private double maxTimeOut; - - private PIDController wheelController; - private double wheelP; - private double wheelI; - private double wheelD; - private double motorVal; - private double target; - - public StartUpFlyWheel(double speed, double maxTimeOut) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - requires(shooter); - - this.wheelP = this.shooter.wheelP; - this.wheelI = this.shooter.wheelI; - this.wheelD = this.shooter.wheelD; - this.wheelController = new PIDController(this.wheelP, this.wheelI, - this.wheelD); - this.wheelController.setDoneRange(0.5); - this.wheelController.setMaxOutput(1.0); - this.wheelController.setMinDoneCycles(3); - this.target = speed; - } - - // Called just before this Command runs the first time - protected void initialize() { - this.wheelController.setSetPoint(this.target); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - double shooterSpeed = this.wheelController - .calcPID(this.shooter.getShooterSpeed()); - - this.shooter.setFlyWheelMotorVal(shooterSpeed); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean isDone = this.wheelController.isDone(); - if (timeSinceInitialized() >= maxTimeOut || isDone) { - System.out.println("time: " + timeSinceInitialized()); - return true; - } - return false; - } - - // Called once after isFinished returns true - protected void end() { - this.shooter.stopFlyWheel(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index f39f1ff..150ce50 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -7,13 +7,13 @@ import com.ctre.CANTalon; import edu.wpi.first.wpilibj.command.Subsystem; public class Shooter extends Subsystem { - public double wheelP = 0.005, wheelI = 0.001, wheelD = -0.003; + public double wheelP = 0, wheelI = 0, wheelD = -0; private static Shooter shooter; private final CANTalon flyWheel, indexWheel; public static final double DEFAULT_INDEXING_SPEED = 0; public static final double DEFAULT_SHOOTING_SPEED = 0; - public static double CURRENT_SHOOTING_SPEED; + public static double CURRENT_SHOOTING_SPEED = DEFAULT_SHOOTING_SPEED; public static final double SHOOTING_SPEED_INCREMENT = 0; @@ -75,6 +75,6 @@ public class Shooter extends Subsystem { } public double getShooterSpeed() { - return 1.0; + return 0.0; } } -- 2.30.2