From ba9f0b126afd5973b11a22dd6640d8d6f0822f5a Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Sat, 18 Mar 2017 18:05:54 -0700 Subject: [PATCH] implement tank drive and toggle winch --- .../usfirst/frc/team3501/robot/Constants.java | 2 + src/org/usfirst/frc/team3501/robot/OI.java | 5 ++ .../robot/commands/climber/ToggleWinch.java | 46 +++++++++++++++++++ .../robot/commands/driving/JoystickDrive.java | 11 +++-- .../team3501/robot/subsystems/Climber.java | 5 +- .../team3501/robot/subsystems/DriveTrain.java | 4 ++ 6 files changed, 66 insertions(+), 7 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 840c497..07b2028 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -31,6 +31,8 @@ public class Constants { public static final int DECREASE_SHOOTER_SPEED_PORT = 7; public static final int RESET_SHOOTER_SPEED_PORT = 5; public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 2; + + public static final int CLIMB_PORT = 0; } public static class Shooter { diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index e9c0552..7002a58 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -2,6 +2,7 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commands.climber.BrakeCANTalons; import org.usfirst.frc.team3501.robot.commands.climber.CoastCANTalons; +import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch; 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; @@ -41,6 +42,7 @@ public class OI { public static Button brakeCANTalons; public static Button coastCANTalons; + public static Button climb; public OI() { leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT); @@ -97,6 +99,9 @@ public class OI { coastCANTalons = new JoystickButton(rightJoystick, Constants.OI.COAST_CANTALONS_PORT); coastCANTalons.whenPressed(new CoastCANTalons()); + + climb = new JoystickButton(leftJoystick, Constants.OI.CLIMB_PORT); + climb.whenPressed(new ToggleWinch()); } public static OI getOI() { diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java new file mode 100644 index 0000000..47e464c --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java @@ -0,0 +1,46 @@ +package org.usfirst.frc.team3501.robot.commands.climber; + +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Climber; + +import edu.wpi.first.wpilibj.command.Command; + +public class ToggleWinch extends Command { + Climber climber = Robot.getClimber(); + private double climbingSpeed; + + public ToggleWinch() { + requires(climber); + climbingSpeed = climber.CLIMBER_SPEED; + } + + @Override + protected void initialize() { + } + + @Override + protected void execute() { + if (climber.shouldBeClimbing) { + climber.setCANTalonsBrakeMode(climber.COAST_MODE); + climber.setMotorValues(climbingSpeed); + } else { + climber.setCANTalonsBrakeMode(climber.BRAKE_MODE); + end(); + } + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + climber.shouldBeClimbing = !climber.shouldBeClimbing; + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java index d1e0f9c..9007a54 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -22,10 +22,13 @@ public class JoystickDrive extends Command { @Override protected void execute() { - final double thrust = OI.rightJoystick.getY(); - final double twist = OI.rightJoystick.getTwist(); - - Robot.getDriveTrain().joystickDrive(-thrust, -twist); + // final double thrust = OI.rightJoystick.getY(); + // final double twist = OI.rightJoystick.getTwist(); + // + // Robot.getDriveTrain().joystickDrive(-thrust, -twist); + double left = OI.leftJoystick.getY(); + double right = OI.rightJoystick.getY(); + Robot.getDriveTrain().tankDrive(left, right); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java b/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java index 4cb6c0b..d9268cb 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java @@ -2,7 +2,6 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.MathLib; -import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous; import com.ctre.CANTalon; @@ -14,7 +13,8 @@ public class Climber extends Subsystem { public static final boolean BRAKE_MODE = true; public static final boolean COAST_MODE = false; - public static final double CLIMBER_SPEED = 0; + public static final double CLIMBER_SPEED = 1.0; + public boolean shouldBeClimbing = false; private CANTalon winch; @@ -43,6 +43,5 @@ public class Climber extends Subsystem { @Override protected void initDefaultCommand() { - setDefaultCommand(new RunWinchContinuous()); } } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 383abd6..f3df881 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -97,6 +97,10 @@ public class DriveTrain extends Subsystem { robotDrive.arcadeDrive(thrust, twist, true); } + public void tankDrive(double left, double right) { + robotDrive.tankDrive(left, right); + } + public void stop() { setMotorValues(0, 0); } -- 2.30.2