added more auto routines

src/org/usfirst/frc/team2059/robot/Robot.java

@@ -17,7 +17,7 @@ public class Robot extends IterativeRobot {
     chooser = new SendableChooser();
 
     chooser.addDefault("Time based drive", new RoutineDriveTime());
-    chooser.addObject("Distance based drive", new RoutineDriveTime());
+    chooser.addObject("Time based defense", new RoutineDefenseTime());
 
     SmartDashboard.putData("Auto mode", chooser);
     SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java

@@ -11,7 +11,7 @@ public class AutoDriveTime extends CommandBase {
   protected void initialize() {
   }
   protected void execute() {
-    driveBase.driveArcade(0,power,0,0);
+    driveBase.driveArcade(0,-power,0,0);
   }
   protected void end() {
     driveBase.stopDriving();

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java

@@ -0,0 +1,37 @@
+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 AutoResetLower extends CommandBase {
+  double speed;
+  public AutoResetLower(double s) {
+    requires(mainArm);
+    speed = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    System.out.println("test");
+    mainArm.disable();
+    mainArm.resetLower(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when bottom limit switch is hit
+    return mainArm.getBottomPressed();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.moveArm(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import org.usfirst.frc.team2059.robot.RobotMap;
+/**
+ *
+ */
+public class AutoSetArmPosition extends CommandBase {
+  double pos;
+  public AutoSetArmPosition(double p) {
+    requires(mainArm);
+    pos = p;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    mainArm.enable();
+    mainArm.setSetpoint(pos);
+    System.out.println(pos-mainArm.getDegrees());
+    System.out.println(mainArm.getDegrees());
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    //if(Math.abs(pos-mainArm.getDegrees()) <= 1){
+    //  return false;
+    //}
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.disable();
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java

@@ -0,0 +1,13 @@
+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 RoutineDefenseTime extends CommandGroup{
+   public RoutineDefenseTime(){
+      addSequential(new AutoResetLower(-1));
+      addSequential(new AutoSetArmPosition(15));
+      //Extend stop
+      addSequential(new AutoSetArmPosition(10));
+      addSequential(new AutoDriveTime(2,.5));
+   }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java

@@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
 import org.usfirst.frc.team2059.robot.Robot;
 public class RoutineDriveTime extends CommandGroup{
    public RoutineDriveTime(){
+      addSequential(new AutoResetLower(-1));
       addSequential(new AutoDriveTime(2,.5));
    }
 }

src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java

@@ -28,7 +28,8 @@ public class SetArmPosition extends CommandBase {
   // Make this return true when this Command no longer needs to run execute()
   protected boolean isFinished() {
     // Stop when either limit switch is hit
-    return mainArm.getBottomPressed() || mainArm.getTopPressed();
+    //return mainArm.getBottomPressed() || mainArm.getTopPressed();
+    return false;
   }
   // Called once after isFinished returns true
   protected void end() {

src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java

@@ -41,11 +41,9 @@ public class MainArm extends PIDSubsystem {
   }
   public void resetLower(double speed) {
     if (getBottomPressed()) {
-      System.out.println("PRESSDE");
       moveArm(0);
       return;
     } else {
-      System.out.println("not pressed");
       moveArm(speed);
     }
   }
@@ -66,7 +64,7 @@ public class MainArm extends PIDSubsystem {
     return !limitSwitchTop.get();
   }
   private double potToDegrees(double pot) {
-    System.out.println((pot - min) / (Math.abs(min - max) / 90));
+    //System.out.println((pot - min) / (Math.abs(min - max) / 90));
     return (pot - min) / (Math.abs(min - max) / 90);
   }
   /**
@@ -75,9 +73,9 @@ public class MainArm extends PIDSubsystem {
    */
   private boolean calibrate() {
     // It can't be calibrated if the limit swithces are disabled
-    if (!SmartDashboard.getBoolean("UseLimitSwitches")) {
-      return false;
-    }
+    //if (!SmartDashboard.getBoolean("UseLimitSwitches")) {
+    //  return false;
+    //}
     if (getBottomPressed()) {
       System.out.println("Calibrating bottom to: " + getRaw());
       min = getRaw();