From bf44c95b23230185744201ab521353275aa10f32 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Thu, 13 Mar 2014 18:24:42 -0700
Subject: [PATCH] Revert "Committed after first practice match. We now have a
 working Auto sequence 2"

This reverts commit 30dfdd0f22a7207baffade6f124fbeefc8bd58e6.
---
 MyRobot.cpp | 22 +++++++++-------------
 1 file changed, 9 insertions(+), 13 deletions(-)

diff --git a/MyRobot.cpp b/MyRobot.cpp
index d7e7e11..0052775 100644
--- a/MyRobot.cpp
+++ b/MyRobot.cpp
@@ -80,19 +80,16 @@ public:
     SmartDashboard::PutNumber("Throttle", throttle);
     SmartDashboard::PutNumber("upLimit", 120.0f);
     SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
-    SmartDashboard::PutNumber("Log Level", 1.0f);
+    SmartDashboard::PutNumber("Log Level", 1);
     //Ultrasonic
     SmartDashboard::PutNumber("Wall Left", voltToDistance(WallSonicLeft.GetAverageVoltage(),true));
     SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true));
     SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
     SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
     //Autonomous values
-    SmartDashboard::PutNumber("Auto Distance",70.0f);
-    SmartDashboard::PutNumber("Collector Speed",1.0f);
     SmartDashboard::PutNumber("AutoPower",0.455f);
     SmartDashboard::PutNumber("AutoCorrection",0.06f);
-    SmartDashboard::PutNumber("Initial Drive Delay",2.0f);
-    SmartDashboard::PutNumber("Inital Drive Timeout", 4.5f);
+    SmartDashboard::PutNumber("Inital Drive Timeout", 2.5f);
     SmartDashboard::PutNumber("First Shot Start", 0.5f);
     SmartDashboard::PutNumber("First Shot Stop", 1.0f);
     SmartDashboard::PutNumber("Reverse direction start",0.0f);
@@ -102,7 +99,7 @@ public:
     SmartDashboard::PutNumber("Second Shot Start", 0.5f);
     SmartDashboard::PutNumber("Second Shot Stop", 1.0f);
     SmartDashboard::PutNumber("Autonomous step",0.0f);
-    SmartDashboard::PutNumber("Autonomous sequence", 2.0f);
+    SmartDashboard::PutNumber("Autonomous sequence", 0.0f);
     //Shooter presets
     SmartDashboard::PutNumber("ShortRange",0.465f); //Power for the shooter when against the low goal
     SmartDashboard::PutNumber("ShooterButtonPower10",0.605f);
@@ -274,7 +271,7 @@ public:
         //Autonomous0{{{
         //Drive{{{
         if(currentStep==0){
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
             driveRobot(-1.0f,correction);
           }else{
             driveRobot(0.0f,0.0f);
@@ -304,6 +301,7 @@ public:
         //}}}
         //Lower Shooter{{{
         if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
+          driveRobot(-1.0f,correction);
           if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
             shootRobot(-0.3f);
           }else{
@@ -321,7 +319,7 @@ public:
         //Autonomous1{{{
         //Drive{{{
         if(currentStep==0){
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
             driveRobot(-1.0f,correction);
           }else{
             driveRobot(0.0f,0.0f);
@@ -367,7 +365,7 @@ public:
         //}}}
         //Drive{{{
         if(currentStep==3&&c>SmartDashboard::GetNumber("Second Drive Start")*200){
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
             driveRobot(-1.0f,correction);
           }else{
             driveRobot(0.0f,0.0f);
@@ -399,15 +397,13 @@ public:
       }else if(SmartDashboard::GetNumber("Autonomous sequence")==2){
         //Autonomous2{{{
         //Drive{{{
-        if(currentStep==0&&c>SmartDashboard::GetNumber("Initial Drive Delay")*200){
-          setMotorValue(6, 1, SmartDashboard::GetNumber("Collector Speed"));
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+        if(currentStep==0){
+          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
             driveRobot(-1.0f,correction);
           }else{
             driveRobot(0.0f,0.0f);
           }
           if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
-            setMotorValue(6, 1, 0);
             driveRobot(0.0f,0.0f);
             currentStep++;
             c=0;