From: Cindy Zhang Date: Fri, 10 Mar 2017 21:11:06 +0000 (-0800) Subject: add necessary commands/buttons X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=fc01fb0fb74d31a0818c69d0306253deb4236c58 add necessary commands/buttons --- diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index d51dfe7..f0cc6e2 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -14,18 +14,22 @@ public class Constants { public static class OI { public final static int LEFT_STICK_PORT = 0; public final static int RIGHT_STICK_PORT = 1; + public static final int GAME_PAD_PORT = 2; - // Need to fill in the port numbers of the following buttons - public final static int TOGGLE_FLYWHEEL_PORT = 4; - public final static int RUN_INDEXWHEEL_PORT = 1; - public final static int REVERSE_INDEXWHEEL_PORT = 2; public final static int TOGGLE_GEAR_PORT = 5; public final static int RUN_INTAKE_PORT = 1; public final static int REVERSE_INTAKE_PORT = 4; - public static final int INCREASE_SHOOTER_SPEED_PORT = 6; - public static final int DECREASE_SHOOTER_SPEED_PORT = 2; + + public final static int RUN_INDEXWHEEL_PORT = 1; + public final static int REVERSE_INDEXWHEEL_PORT = 2; public static final int BRAKE_CANTALONS_PORT = 5; public static final int COAST_CANTALONS_PORT = 3; + + public final static int TOGGLE_FLYWHEEL_PORT = 4; + public static final int REVERSE_FLYWHEEL_PORT = 1; + public static final int INCREASE_SHOOTER_SPEED_PORT = 6; + public static final int DECREASE_SHOOTER_SPEED_PORT = 2; + public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 3; } public static class Shooter { @@ -43,12 +47,14 @@ public class Constants { } public static class DriveTrain { - // GEARS - public static final int PISTON_MODULE = 10, LEFT_GEAR_PISTON_FORWARD = 0, + public static final int PISTON_MODULE = 10; + public static final int GEAR_MANIPULATOR_PISTON_FORWARD = 4, + GEAR_MANIPULATOR_PISTON_REVERSE = 5; + public static final int LEFT_GEAR_PISTON_FORWARD = 0, LEFT_GEAR_PISTON_REVERSE = 1, RIGHT_GEAR_PISTON_FORWARD = 3, RIGHT_GEAR_PISTON_REVERSE = 2; - public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward; - public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse; + public static final Value FORWARD_PISTON_VALUE = DoubleSolenoid.Value.kForward; + public static final Value REVERSE_PISTON_VALUE = DoubleSolenoid.Value.kReverse; // MOTOR CONTROLLERS public static final int FRONT_LEFT = 1; diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index a1abe0a..9f8ea7a 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -2,11 +2,13 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commands.driving.BrakeCANTalons; import org.usfirst.frc.team3501.robot.commands.driving.CoastCANTalons; -import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear; +import org.usfirst.frc.team3501.robot.commands.driving.ToggleDriveGear; +import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston; import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous; import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.DecreaseShootingSpeed; import org.usfirst.frc.team3501.robot.commands.shooter.IncreaseShootingSpeed; +import org.usfirst.frc.team3501.robot.commands.shooter.ReverseFlyWheelContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous; @@ -19,12 +21,15 @@ public class OI { private static OI oi; public static Joystick leftJoystick; public static Joystick rightJoystick; + public static Joystick gamePad; public static Button runIndexWheel; public static Button reverseIndexWheel; public static Button toggleFlyWheel; + public static Button reverseFlyWheel; public static Button toggleGear; + public static Button toggleGearManipulatorPiston; public static Button runIntake; public static Button reverseIntake; @@ -38,6 +43,7 @@ public class OI { public OI() { leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT); rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT); + gamePad = new Joystick(Constants.OI.GAME_PAD_PORT); runIndexWheel = new JoystickButton(rightJoystick, Constants.OI.RUN_INDEXWHEEL_PORT); @@ -47,13 +53,21 @@ public class OI { Constants.OI.REVERSE_INDEXWHEEL_PORT); reverseIndexWheel.whileHeld(new ReverseIndexWheelContinuous()); - toggleFlyWheel = new JoystickButton(rightJoystick, + toggleFlyWheel = new JoystickButton(gamePad, Constants.OI.TOGGLE_FLYWHEEL_PORT); toggleFlyWheel.toggleWhenPressed(new RunFlyWheelContinuous()); + reverseFlyWheel = new JoystickButton(gamePad, + Constants.OI.REVERSE_FLYWHEEL_PORT); + reverseFlyWheel.whileHeld(new ReverseFlyWheelContinuous()); + toggleGear = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_GEAR_PORT); - toggleGear.whenPressed(new ToggleGear()); + toggleGear.whenPressed(new ToggleDriveGear()); + + toggleGearManipulatorPiston = new JoystickButton(gamePad, + Constants.OI.TOGGLE_GEAR_MANIPULATOR_PORT); + toggleGearManipulatorPiston.whenPressed(new ToggleGearManipulatorPiston()); runIntake = new JoystickButton(leftJoystick, Constants.OI.RUN_INTAKE_PORT); runIntake.whileHeld(new RunIntakeContinuous()); @@ -62,11 +76,11 @@ public class OI { Constants.OI.REVERSE_INTAKE_PORT); reverseIntake.whileHeld(new ReverseIntakeContinuous()); - increaseShooterSpeed = new JoystickButton(leftJoystick, + increaseShooterSpeed = new JoystickButton(gamePad, Constants.OI.INCREASE_SHOOTER_SPEED_PORT); increaseShooterSpeed.whenPressed(new IncreaseShootingSpeed()); - decreaseShooterSpeed = new JoystickButton(leftJoystick, + decreaseShooterSpeed = new JoystickButton(gamePad, Constants.OI.DECREASE_SHOOTER_SPEED_PORT); decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed()); diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index a8c4864..a9fb3bf 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -67,7 +67,7 @@ public class Robot extends IterativeRobot { @Override public void teleopPeriodic() { - // driveTrain.printEncoderOutput(); + driveTrain.printEncoderOutput(); Scheduler.getInstance().run(); updateSmartDashboard(); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java new file mode 100644 index 0000000..ee15ec8 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java @@ -0,0 +1,51 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; + +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.command.Command; + +/** + * This command toggles the gear(low or high). + * + * post-condition: if the drivetrain is running at high gear, it will be made to + * run at low gear, and vice versa + */ +public class ToggleDriveGear extends Command { + DriveTrain driveTrain = Robot.getDriveTrain(); + + public ToggleDriveGear() { + requires(driveTrain); + } + + @Override + protected void initialize() { + + } + + @Override + protected void execute() { + Value leftPistonValue = driveTrain.getLeftDriveTrainPiston(); + Value rightPistonValue = driveTrain.getRightDriveTrainPiston(); + if (leftPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) { + driveTrain.setHighGear(); + } else { + driveTrain.setLowGear(); + } + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + } + + @Override + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java deleted file mode 100644 index ce74890..0000000 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java +++ /dev/null @@ -1,51 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.driving; - -import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; - -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import edu.wpi.first.wpilibj.command.Command; - -/** - * This command toggles the gear(low or high). - * - * post-condition: if the drivetrain is running at high gear, it will be made to - * run at low gear, and vice versa - */ -public class ToggleGear extends Command { - DriveTrain driveTrain = Robot.getDriveTrain(); - - public ToggleGear() { - requires(driveTrain); - } - - @Override - protected void initialize() { - - } - - @Override - protected void execute() { - Value leftGearPistonValue = driveTrain.getLeftGearPistonValue(); - Value rightGearPistonValue = driveTrain.getRightGearPistonValue(); - if (leftGearPistonValue == Constants.DriveTrain.LOW_GEAR) { - driveTrain.setHighGear(); - } else { - driveTrain.setLowGear(); - } - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - } - - @Override - protected void interrupted() { - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGearManipulatorPiston.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGearManipulatorPiston.java new file mode 100644 index 0000000..eed7f14 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGearManipulatorPiston.java @@ -0,0 +1,49 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; + +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ToggleGearManipulatorPiston extends Command { + private DriveTrain driveTrain = Robot.getDriveTrain(); + + public ToggleGearManipulatorPiston() { + } + + @Override + protected void initialize() { + Value gearPistonValue = driveTrain.getGearManipulatorPistonValue(); + if (gearPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) { + driveTrain.extendGearManipulatorPiston(); + } else { + driveTrain.retractGearManipulatorPiston(); + } + } + + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseFlyWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseFlyWheelContinuous.java new file mode 100644 index 0000000..dd4f149 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseFlyWheelContinuous.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team3501.robot.commands.shooter; + +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Shooter; + +import edu.wpi.first.wpilibj.command.Command; + +public class ReverseFlyWheelContinuous extends Command { + private Shooter shooter = Robot.getShooter(); + + public ReverseFlyWheelContinuous() { + } + + @Override + protected void initialize() { + } + + @Override + protected void execute() { + shooter.reverseFlyWheel(); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + shooter.stopFlyWheel(); + } + + @Override + protected void interrupted() { + end(); + } + +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java index 9594013..815577e 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java @@ -1,6 +1,5 @@ package org.usfirst.frc.team3501.robot.commands.shooter; -import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.Robot; import org.usfirst.frc.team3501.robot.subsystems.Shooter; @@ -34,14 +33,6 @@ public class RunIndexWheelContinuous extends Command { @Override protected void execute() { - if (timeSinceInitialized() % 0.5 <= 0.02) { - if (Shooter.getShooter().getPistonValue() == Constants.Shooter.LOW_GEAR) { - Shooter.getShooter().setHighGear(); - } else { - Shooter.getShooter().setLowGear(); - } - } - double shooterSpeed = shooter.getShooterRPM(); double targetShooterSpeed = shooter.getTargetShootingSpeed(); double threshold = shooter.getRPMThreshold(); diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index bad303a..a6a6f8c 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -37,7 +37,8 @@ public class DriveTrain extends Subsystem { private final CANTalon frontLeft, frontRight, rearLeft, rearRight; private final RobotDrive robotDrive; private final Encoder leftEncoder, rightEncoder; - private final DoubleSolenoid leftGearPiston, rightGearPiston; + private final DoubleSolenoid leftDriveTrainPiston, rightDriveTrainPiston; + private final DoubleSolenoid gearManipulatorPiston; private ADXRS450_Gyro imu; @@ -65,12 +66,19 @@ public class DriveTrain extends Subsystem { this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT); // TODO: Not sure if MODULE_NUMBER should be the same for both - leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.PISTON_MODULE, + leftDriveTrainPiston = new DoubleSolenoid( + Constants.DriveTrain.PISTON_MODULE, Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD, Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE); - rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.PISTON_MODULE, + rightDriveTrainPiston = new DoubleSolenoid( + Constants.DriveTrain.PISTON_MODULE, Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD, Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE); + + gearManipulatorPiston = new DoubleSolenoid( + Constants.DriveTrain.PISTON_MODULE, + Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_FORWARD, + Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_REVERSE); } public static DriveTrain getDriveTrain() { @@ -158,38 +166,50 @@ public class DriveTrain extends Subsystem { * @return a value that is the current setpoint for the piston kReverse or * KForward */ - public Value getLeftGearPistonValue() { - return leftGearPiston.get(); + public Value getLeftDriveTrainPiston() { + return leftDriveTrainPiston.get(); } /* * @return a value that is the current setpoint for the piston kReverse or * KForward */ - public Value getRightGearPistonValue() { - return rightGearPiston.get(); + public Value getRightDriveTrainPiston() { + return rightDriveTrainPiston.get(); } /* * Changes the ball shift gear assembly to high */ public void setHighGear() { - changeGear(Constants.DriveTrain.HIGH_GEAR); + changeGear(Constants.DriveTrain.FORWARD_PISTON_VALUE); } /* * Changes the ball shift gear assembly to low */ public void setLowGear() { - changeGear(Constants.DriveTrain.LOW_GEAR); + changeGear(Constants.DriveTrain.REVERSE_PISTON_VALUE); } /* * Changes the gear to a DoubleSolenoid.Value */ private void changeGear(DoubleSolenoid.Value gear) { - leftGearPiston.set(gear); - rightGearPiston.set(gear); + leftDriveTrainPiston.set(gear); + rightDriveTrainPiston.set(gear); + } + + public Value getGearManipulatorPistonValue() { + return gearManipulatorPiston.get(); + } + + public void extendGearManipulatorPiston() { + gearManipulatorPiston.set(Constants.DriveTrain.FORWARD_PISTON_VALUE); + } + + public void retractGearManipulatorPiston() { + gearManipulatorPiston.set(Constants.DriveTrain.REVERSE_PISTON_VALUE); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index faca5dd..85ecf46 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -20,8 +20,9 @@ public class Shooter extends Subsystem { private PIDController wheelController; private static final double RPM_THRESHOLD = 10; - private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75; - private static final double DEFAULT_SHOOTING_SPEED = 3100; // rpm + private static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75; + private static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75; + private static final double DEFAULT_SHOOTING_SPEED = 2700; // rpm private static final double SHOOTING_SPEED_INCREMENT = 50; private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; @@ -163,4 +164,8 @@ public class Shooter extends Subsystem { private void changeGear(DoubleSolenoid.Value gear) { piston.set(gear); } + + public void reverseFlyWheel() { + this.setFlyWheelMotorVal(shooter.REVERSE_FLYWHEEL_MOTOR_VALUE); + } }