From cef1f36da71950bc32d74d2ba0477a51f478f209 Mon Sep 17 00:00:00 2001 From: Rohan Rodrigues Date: Tue, 7 Feb 2017 16:46:11 -0800 Subject: [PATCH] Add JoystickButton objects and constants for Drivetrain, Climer, Intake, and Shooter --- .../usfirst/frc/team3501/robot/Constants.java | 11 +++++ src/org/usfirst/frc/team3501/robot/OI.java | 45 +++++++++++++++++-- .../commands/climber/RunWinchContinuous.java | 8 ++-- .../team3501/robot/subsystems/DriveTrain.java | 13 ++++++ 4 files changed, 69 insertions(+), 8 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 3933fdf..a1a8cb0 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -14,10 +14,15 @@ public class Constants { public static class OI { public final static int LEFT_STICK_PORT = 0; public final static int RIGHT_STICK_PORT = 1; + + // Need to fill in the port numbers of the following buttons public final static int TOGGLE_WINCH_PORT = 0; public final static int TOGGLE_FLYWHEEL_PORT = 0; public final static int TOGGLE_INDEXWHEEL_PORT = 0; + public final static int REVERSE_INDEXWHEEL_PORT = 0; public final static int TOGGLE_GEAR_PORT = 0; + public final static int TOGGLE_INTAKE_PORT = 0; + public final static int REVERSE_INTAKE_PORT = 0; } public static class Shooter { @@ -40,6 +45,8 @@ public class Constants { public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward; public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse; + public static final double CLIMBER_SPEED = 5; + // MOTOR CONTROLLERS public static final int FRONT_LEFT = 1; public static final int FRONT_RIGHT = 3; @@ -53,11 +60,15 @@ public class Constants { public static final int ENCODER_RIGHT_B = 3; public static final SPI.Port GYRO_PORT = SPI.Port.kOnboardCS0; + + public final static int TRIGGER_DRIVE_PORT = 0; } public static class Intake { public static final int INTAKE_ROLLER_PORT = 0; + public final static int HOLD_INTAKE_PORT = 0; + } public static enum Direction { diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index ec405ac..32ec062 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -1,6 +1,13 @@ package org.usfirst.frc.team3501.robot; +import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition; +import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous; import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear; +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.ReverseIndexWheelContinuous; +import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous; +import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; @@ -11,24 +18,54 @@ public class OI { public static Joystick leftJoystick; public static Joystick rightJoystick; public static Button toggleWinch; + private boolean isClimbing = false; - public static Button toggleIndexWheel; + public static Button runIndexWheel; + public static Button reverseIndexWheel; public static Button toggleFlyWheel; public static Button toggleGear; + public static Button runIntake; + public static Button reverseIntake; + public OI() { leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT); rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT); - toggleWinch = new JoystickButton(leftJoystick, - Constants.OI.TOGGLE_WINCH_PORT); - toggleIndexWheel = new JoystickButton(leftJoystick, + + runIndexWheel = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_INDEXWHEEL_PORT); + runIndexWheel.whileHeld(new RunIndexWheelContinuous()); + + reverseIndexWheel = new JoystickButton(leftJoystick, + Constants.OI.REVERSE_INDEXWHEEL_PORT); + reverseIndexWheel.whileHeld(new ReverseIndexWheelContinuous()); + toggleFlyWheel = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_FLYWHEEL_PORT); + toggleFlyWheel.toggleWhenPressed(new RunFlyWheelContinuous()); + toggleGear = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_GEAR_PORT); toggleGear.whenPressed(new ToggleGear()); + + runIntake = new JoystickButton(leftJoystick, + Constants.OI.TOGGLE_INTAKE_PORT); + runIntake.whileHeld(new RunIntakeContinuous()); + + reverseIntake = new JoystickButton(leftJoystick, + Constants.OI.REVERSE_INTAKE_PORT); + reverseIntake.whileHeld(new ReverseIntakeContinuous()); + + toggleWinch = new JoystickButton(leftJoystick, + Constants.OI.TOGGLE_WINCH_PORT); + if (!isClimbing) { + toggleWinch.whenPressed(new RunWinchContinuous()); + isClimbing = true; + } else { + toggleWinch.whenPressed(new MaintainClimbedPosition()); + isClimbing = false; + } } public static OI getOI() { diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java index e201210..6527799 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class RunWinchContinuous extends Command { - private double motorVal; /** * See JavaDoc comment in class for details @@ -25,14 +24,15 @@ public class RunWinchContinuous extends Command { * @param motorVal * value range is from -1 to 1 */ - public RunWinchContinuous(double motorVal) { + public RunWinchContinuous() { requires(Robot.getDriveTrain()); - this.motorVal = motorVal; } @Override protected void initialize() { - Robot.getDriveTrain().setMotorValues(motorVal, motorVal); + Robot.getDriveTrain().setMotorValues( + Robot.getDriveTrain().getClimbingSpeed(), + Robot.getDriveTrain().getClimbingSpeed()); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index f7c35a9..f30d4c2 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -32,6 +32,9 @@ public class DriveTrain extends Subsystem { private ADXRS450_Gyro imu; + private boolean isClimbing; + private static double CLIMBER_SPEED;; + private DriveTrain() { // MOTOR CONTROLLERS frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); @@ -60,6 +63,8 @@ public class DriveTrain extends Subsystem { rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER, Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD, Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE); + + CLIMBER_SPEED = Constants.DriveTrain.CLIMBER_SPEED; } public static DriveTrain getDriveTrain() { @@ -183,4 +188,12 @@ public class DriveTrain extends Subsystem { setDefaultCommand(new JoystickDrive()); } + public boolean isClimbing() { + return this.isClimbing; + } + + public double getClimbingSpeed() { + return this.CLIMBER_SPEED; + } + } -- 2.30.2