From 54c588d95fe6aa0ab8d9a81643e73a9af6c7a85e Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Sun, 14 Feb 2016 18:12:44 -0800 Subject: [PATCH] add commands to digital buttons in OI and create/edit necessary commands --- .../usfirst/frc/team3501/robot/Constants.java | 2 ++ src/org/usfirst/frc/team3501/robot/OI.java | 21 ++++++++++++++----- .../robot/commands/driving/Turn180.java | 16 ++++++++++++++ .../team3501/robot/subsystems/DriveTrain.java | 9 ++++++-- .../team3501/robot/subsystems/IntakeArm.java | 3 ++- 5 files changed, 43 insertions(+), 8 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 66a37d8f..59bbcca3 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -69,6 +69,8 @@ public class Constants { public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward; public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse; + public static boolean inverted = false; + } public static class Scaler { diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index adb37e40..4cd6450f 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -1,16 +1,20 @@ package org.usfirst.frc.team3501.robot; +import org.usfirst.frc.team3501.robot.commands.auton.CompactRobot; import org.usfirst.frc.team3501.robot.commands.auton.PassChevalDeFrise; import org.usfirst.frc.team3501.robot.commands.auton.PassDrawBridge; import org.usfirst.frc.team3501.robot.commands.auton.PassPortcullis; import org.usfirst.frc.team3501.robot.commands.auton.PassSallyPort; +import org.usfirst.frc.team3501.robot.commands.driving.Turn180; import org.usfirst.frc.team3501.robot.commands.intakearm.IntakeBall; +import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArmToAngle; import org.usfirst.frc.team3501.robot.commands.scaler.ExtendLift; import org.usfirst.frc.team3501.robot.commands.scaler.RetractLift; import org.usfirst.frc.team3501.robot.commands.scaler.RunWinchContinuous; import org.usfirst.frc.team3501.robot.commands.scaler.StopWinch; import org.usfirst.frc.team3501.robot.commands.shooter.Shoot; import org.usfirst.frc.team3501.robot.commands.shooter.runShooter; +import org.usfirst.frc.team3501.robot.subsystems.IntakeArm; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Joystick; @@ -98,8 +102,14 @@ public class OI { passDrawbridge.whenPressed(new PassDrawBridge()); passSallyPort.whenPressed(new PassSallyPort()); - lowerChevalDeFrise - .whenPressed(/* TO DO: define this, and fill in commands */); + lowerChevalDeFrise.whenPressed(new MoveIntakeArmToAngle( + IntakeArm.potAngles[0], IntakeArm.moveIntakeArmSpeed)); + moveToIntakeBoulder.whenPressed(new MoveIntakeArmToAngle( + IntakeArm.potAngles[1], IntakeArm.moveIntakeArmSpeed)); + poiseAboveChevalDeFrise.whenPressed(new MoveIntakeArmToAngle( + IntakeArm.potAngles[2], IntakeArm.moveIntakeArmSpeed)); + moveIntakeArmInsideRobot.whenPressed(new MoveIntakeArmToAngle( + IntakeArm.potAngles[3], IntakeArm.moveIntakeArmSpeed)); if (toggleScalingMode.get()) { if (!isScalingMode) { @@ -117,6 +127,7 @@ public class OI { if (!isScalingMode) { toggleShooter.toggleWhenPressed(new runShooter()); compactRobot_1.whenPressed(new CompactRobot()); + compactRobot_2.whenPressed(new CompactRobot()); intakeBoulder.whenPressed(new IntakeBall()); shootBoulder.whenPressed(new Shoot()); @@ -125,13 +136,13 @@ public class OI { toggleShooter.whenPressed(new RunWinchContinuous( Constants.Scaler.WINCH_IN_SPEED)); compactRobot_1.whenPressed(new RetractLift()); + compactRobot_2.whenPressed(new RetractLift()); intakeBoulder.whenReleased(new StopWinch()); shootBoulder.whenPressed(new ExtendLift()); } - SpinRobot180_1 - .whenPressed(/* rotate robot 180, reorient joystick controls */); - + SpinRobot180_1.whenPressed(new Turn180()); + SpinRobot180_2.whenPressed(new Turn180()); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java b/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java new file mode 100644 index 00000000..863d39bd --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java @@ -0,0 +1,16 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Constants; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * + */ +public class Turn180 extends CommandGroup { + + public Turn180() { + addSequential(new TurnForAngle(180, 5)); + Constants.DriveTrain.inverted = !Constants.DriveTrain.inverted; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index de2626be..1282be1b 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -42,7 +42,7 @@ public class DriveTrain extends PIDSubsystem { // Drivetrain specific constants that relate to the PID controllers private final static double Kp = 1.0, Ki = 0.0, Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; + / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; public DriveTrain() { super(kp, ki, kd); @@ -74,6 +74,7 @@ public class DriveTrain extends PIDSubsystem { +Constants.DriveTrain.LEFT_REVERSE); rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); + Constants.DriveTrain.inverted = false; } @Override @@ -220,8 +221,12 @@ public class DriveTrain extends PIDSubsystem { } public void drive(double left, double right) { - robotDrive.tankDrive(-left, -right); // dunno why but inverted drive (- values is forward) + if (!Constants.DriveTrain.inverted) + robotDrive.tankDrive(-left, + -right); + else + robotDrive.tankDrive(right, left); } public void driveDistance(double dist, double maxTimeOut) { diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java index a9e9ecd5..5e00d05f 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java @@ -23,7 +23,8 @@ public class IntakeArm extends Subsystem { private CANTalon intakeRoller; private CANTalon intakeArm; private AnalogPotentiometer intakePot; - private double[] potAngles = { 0, 45, 90 }; + public static double[] potAngles = { 0, 30, 45, 90 }; // TODO: correct angles + public static double moveIntakeArmSpeed = 0; public IntakeArm() { intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT); -- 2.30.2