From f7b7eb47d2189901c012ea2c9bd318d631f5c3d6 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Wed, 21 Sep 2016 18:28:42 +0000
Subject: [PATCH] added basic time drive auto

---
 src/org/usfirst/frc/team2059/robot/Robot.java |  5 ++++
 .../commands/autonomous/AutoDriveTime.java    | 26 +++++++++++++++++++
 .../commands/autonomous/RoutineDriveTime.java |  9 +++++++
 3 files changed, 40 insertions(+)
 create mode 100644 src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java
 create mode 100644 src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java

diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java
index efac689..96bea3a 100644
--- a/src/org/usfirst/frc/team2059/robot/Robot.java
+++ b/src/org/usfirst/frc/team2059/robot/Robot.java
@@ -1,5 +1,6 @@
 package org.usfirst.frc.team2059.robot;
 import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.commands.autonomous.*;
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -14,6 +15,10 @@ public class Robot extends IterativeRobot {
     CommandBase.init();
     oi = new OI();
     chooser = new SendableChooser();
+
+    chooser.addDefault("Time based drive", new RoutineDriveTime());
+    chooser.addObject("Distance based drive", new RoutineDriveTime());
+
     SmartDashboard.putData("Auto mode", chooser);
     SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
     SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java
new file mode 100644
index 0000000..55c357f
--- /dev/null
+++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+public class AutoDriveTime extends CommandBase {
+  double power;
+  public AutoDriveTime(double timeout, double power) {
+    requires(driveBase);
+    this.power=power;
+    setTimeout(timeout);
+  }
+  protected void initialize() {
+  }
+  protected void execute() {
+    driveBase.driveArcade(0,power,0,0);
+  }
+  protected void end() {
+    driveBase.stopDriving();
+  }
+  protected void interrupted() {
+    end();
+  }
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+}
+// vim: sw=2:ts=2:sts=2
diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java
new file mode 100644
index 0000000..0cf108d
--- /dev/null
+++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java
@@ -0,0 +1,9 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineDriveTime extends CommandGroup{
+   public RoutineDriveTime(){
+      addSequential(new AutoDriveTime(2,.5));
+   }
+}