add hella auton

diff --git a/src/org/usfirst/frc/team3501/robot/Joystick.java b/src/org/usfirst/frc/team3501/robot/Joystick.java
index f78be053bfd94622e0f11a9789b0bf24589de6ed..fab679234f9c1486cd4a427da34aab418a786bd2 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Joystick.java
+++ b/src/org/usfirst/frc/team3501/robot/Joystick.java
@@ -29,4 +29,8 @@ public class Joystick extends edu.wpi.first.wpilibj.Joystick {
     public void whileHeld(int button, Command c) {
         buttons.get(button).whileHeld(c);
     }
+
+    public boolean get(int button) {
+        return getRawButton(button);
+    }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index f573281d6b4a7bb86328cfec799991cfa8826734..82c4217ab6229cc1dd87f966c5c72c2ed98ec013 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,5 +1,7 @@
 package org.usfirst.frc.team3501.robot;
 
+import java.util.Arrays;
+
 import org.usfirst.frc.team3501.robot.commands.*;
 
 public class OI {
@@ -40,4 +42,8 @@ public class OI {
     public double getTwistR() {
         return right.getTwist();
     }
+
+    public boolean getRightPressed(int... buttons) {
+        return Arrays.stream(buttons).anyMatch(b -> right.get(b));
+    }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 23a43b7812daab2e2bfe1d188ebfcb025f998b7c..633cde60b724bafc01ef82cf73ce10b8d3fb2c58 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -73,8 +73,9 @@ public class Robot extends IterativeRobot {
     private void chooseAuto() {
         autoChooser = new SendableChooser();
 
-        autoChooser.addDefault("Drive over step", new DriveOverStep());
-        autoChooser.addObject("Drive past step",  new DrivePastStep());
+        autoChooser.addDefault("Pick up container", new ContainerOverStep());
+        autoChooser.addObject("Drive over step",    new DriveOverStep());
+        autoChooser.addObject("Drive past step",    new DrivePastStep());
 
         SmartDashboard.putData("Auto Mode", autoChooser);
     }

diff --git a/src/org/usfirst/frc/team3501/robot/RobotMap.java b/src/org/usfirst/frc/team3501/robot/RobotMap.java
index 8cfb7fb99c177da473e8d069096e0f8690a798c6..5e73c6589c89aed2c68501fdba08e96637d7bfd9 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/RobotMap.java
+++ b/src/org/usfirst/frc/team3501/robot/RobotMap.java
@@ -27,5 +27,6 @@ public class RobotMap {
 
     // auton
     public static final double OVER_STEP_TIME = 1.2, OVER_STEP_SPEED = 0.7,
-                               PAST_STEP_TIME = 1.5, PAST_STEP_SPEED = 0.5;
+                               PAST_STEP_TIME = 1.5, PAST_STEP_SPEED = 0.5,
+                               PICKUP_TIME    = 1.4, PICKUP_SPEED = 0.5;
 }

diff --git a/src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java b/src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java
new file mode 100644 (file)
index 0000000..75e592b
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team3501.robot.autons;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class ContainerOverStep extends CommandGroup {
+
+    public ContainerOverStep() {
+        super("ContainerOverStep");
+
+        requires(Robot.drivetrain);
+        requires(Robot.arm);
+        requires(Robot.claw);
+
+        queueCommands();
+    }
+
+    private void queueCommands() {
+        addSequential(new PickUpContainer());
+        addSequential(new DriveOverStep(-1));
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java
index 22986611bc8306cea529d00525f0af25b2aa0990..f70b3d280d81aa5acc0b74e853b9ab58540b3118 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java
+++ b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java
@@ -12,7 +12,13 @@ public class DriveOverStep extends CommandBase {
         requires(drivetrain);
 
         setTimeout(RobotMap.OVER_STEP_TIME);
-        this.speed = RobotMap.OVER_STEP_SPEED;
+        speed = RobotMap.OVER_STEP_SPEED;
+    }
+
+    // TODO: this is an ugly "solution"
+    public DriveOverStep(int coef) {
+        this();
+        this.speed *= coef;
     }
 
     protected void execute() {

diff --git a/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java b/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java
new file mode 100644 (file)
index 0000000..9b08d1f
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team3501.robot.autons;
+
+import org.usfirst.frc.team3501.robot.RobotMap;
+import org.usfirst.frc.team3501.robot.commands.*;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class PickUpContainer extends CommandGroup {
+
+    public PickUpContainer() {
+        super("PickUpContainer");
+
+        queueCommands();
+    }
+
+    private void queueCommands() {
+        addSequential(new CloseClaw());
+        addSequential(new MoveArmFor(RobotMap.PICKUP_TIME, RobotMap.PICKUP_SPEED));
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java b/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java
new file mode 100644 (file)
index 0000000..c1ca507
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class DriveFor extends CommandBase {
+
+    private double speed;
+
+    public DriveFor(double secs, double speed) {
+        super("DriveFor");
+        requires(drivetrain);
+
+        setTimeout(secs);
+        this.speed = speed;
+    }
+
+    protected void execute() {
+        drivetrain.driveRaw(speed, 0);
+    }
+
+    protected boolean isFinished() {
+        return isTimedOut();
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java
index cda6cdd7f35cd96a9d6d3592a0f63c33d7634aac..980214781095e3710b79dcb625ada930be0c2a3a 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java
@@ -8,7 +8,13 @@ public class DriveWithJoysticks extends CommandBase {
     }
 
     protected void execute() {
-        drivetrain.drive(oi.getForwardR(), oi.getTwistR());
+        double forward = oi.getForwardR();
+        double twist   = oi.getTwistR();
+
+        if (oi.getRightPressed(3, 4, 5, 6))
+            twist = 0;
+
+        drivetrain.drive(forward, twist);
     }
 
     protected boolean isFinished() {

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java
index f0865af4069de3bb14f2d92772a34f9b1de14142..2354b3dd73517920d819a89ada92c9da2a1dc8c5 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java
@@ -8,7 +8,7 @@ public class MoveArm extends CommandBase {
     }
 
     protected void execute() {
-        arm.setFromJoystick(oi.getForwardL());
+        arm.setFromJoystick(-oi.getForwardL());
     }
 
     protected boolean isFinished() {

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java
new file mode 100644 (file)
index 0000000..7f475f1
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class MoveArmFor extends CommandBase {
+
+    private double speed;
+
+    public MoveArmFor(double secs, double speed) {
+        super("MoveArmFor");
+        requires(arm);
+
+        setTimeout(secs);
+        this.speed = speed;
+    }
+
+    protected void execute() {
+        arm.set(speed);
+    }
+
+    protected boolean isFinished() {
+        return isTimedOut();
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java
index 72428e4a7ca5e757f42529fbac5adc98c906e5f4..90e78473d6971732bbc741ad769e4ec8bdb12809 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java
@@ -34,6 +34,10 @@ public class Drivetrain extends Subsystem {
                 false);
     }
 
+    public void driveRaw(double forward, double twist) {
+        robotDrive.arcadeDrive(forward, twist, false);
+    }
+
     public void goForward(double speed) {
         robotDrive.arcadeDrive(speed, 0);
     }