From 3668bf318d4fb65ee13e323af9a6203f7f4eb07e Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 4 Apr 2014 20:29:38 -0400
Subject: [PATCH] Fixed half speed in auto and in teleop (Untested)

---
 MyRobot.cpp | 438 ++++++++++++++++++++++++++--------------------------
 1 file changed, 219 insertions(+), 219 deletions(-)

diff --git a/MyRobot.cpp b/MyRobot.cpp
index a1c9395..ac9a812 100644
--- a/MyRobot.cpp
+++ b/MyRobot.cpp
@@ -146,12 +146,12 @@ public:
     }else if(y!=0.0f&&y<-1.0f){
       y=-1.0f;
     }
+    if(SmartDashboard::GetBoolean("HalfPower")){
+      x/=2;
+      y/=2;
+    }
     int leftPower=((y+x)/2+1)*127+1;
     int rightPower=((y-x)/2+1)*127+1;
-    if(SmartDashboard::GetBoolean("HalfPower")){
-      leftPower/=2;
-      rightPower/=2;
-    }
     setMotorValue(1,1,leftPower);
     setMotorValue(2,1,leftPower);
     setMotorValue(3,1,leftPower);
@@ -262,15 +262,15 @@ public:
       compressor.Start();
     }
   }
-    //}}}
-    //updateJoystick{{{
-    int updateJoystick(*stick){
-      for(i=1,i<=12,i++){
-      state[i] = stick.GetRawButton(i);
-      }
-      return state;
+  //}}}
+  //updateJoystick{{{
+  int updateJoystick(*stick){
+    for(i=1,i<=12,i++){
+      state[i]=stick.GetRawButton(i);
     }
-    //}}}
+    return state;
+  }
+  //}}}
   //Autonomous{{{
   void Autonomous(){
     //Initializations{{{
@@ -291,215 +291,216 @@ public:
     //}}}
     while(IsEnabled()&&IsAutonomous()){
       if(!SmartDashboard::GetBoolean("HalfPower")){
-      if(SmartDashboard::GetNumber("Autonomous sequence")==0){
-        //Autonomous0{{{
-        //Drive{{{
-        if(currentStep==0){
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
-            driveRobot(-1.0f,correction);
-          }else{
-            driveRobot(0.0f,0.0f);
+        if(SmartDashboard::GetNumber("Autonomous sequence")==0){
+          //Autonomous0{{{
+          //Drive{{{
+          if(currentStep==0){
+            if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+              driveRobot(-1.0f,correction);
+            }else{
+              driveRobot(0.0f,0.0f);
+            }
+            if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
+              driveRobot(0.0f,0.0f);
+              currentStep++;
+              c=0;
+            }
           }
-          if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
-            driveRobot(0.0f,0.0f);
-            currentStep++;
-            c=0;
+          //}}}
+          //Shoot{{{
+          if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
+            if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(power);
+            }else{
+              shootRobot(0.0f);
+            }
+            setMotorValue(6, 1, 1);
+            if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
+              shootRobot(0.0f);
+              setMotorValue(6, 1, 0);
+              currentStep++;
+              c=0;
+            }
           }
+          //}}}
+          //Lower Shooter{{{
+          if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
+            if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(-0.3f);
+            }else{
+              shootRobot(0.0f);
+            }
+            if(c==(3.0f)*200){
+              shootRobot(0.0f);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //}}}
+        }else if(SmartDashboard::GetNumber("Autonomous sequence")==1){
+          //Autonomous1{{{
+          //Drive{{{
+          if(currentStep==0){
+            if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+              driveRobot(-1.0f,correction);
+            }else{
+              driveRobot(0.0f,0.0f);
+            }
+            if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
+              driveRobot(0.0f,0.0f);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Shoot{{{
+          if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
+            if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(power);
+            }else{
+              shootRobot(0.0f);
+            }
+            setMotorValue(6, 1, 1);
+            if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
+              shootRobot(0.0f);
+              setMotorValue(6, 1, 0);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Drive Backwards{{{
+          if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
+            driveRobot(1.0f,correction);
+            if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(-0.2f);
+            }else{
+              shootRobot(0.0f);
+            }
+            if(c==(SmartDashboard::GetNumber("Reverse direction stop"))*200){
+              driveRobot(0.0f,0.0f);
+              shootRobot(0.0f);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Drive{{{
+          if(currentStep==3&&c>SmartDashboard::GetNumber("Second Drive Start")*200){
+            if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+              driveRobot(-1.0f,correction);
+            }else{
+              driveRobot(0.0f,0.0f);
+            }
+            if(c==(SmartDashboard::GetNumber("Inital Drive Timeout"))*200){
+              driveRobot(0.0f,0.0f);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Shoot{{{
+          if(currentStep==4&&c>(SmartDashboard::GetNumber("Second Shot Start"))*200){
+            if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(power);
+            }else{
+              shootRobot(0.0f);
+            }
+            setMotorValue(6, 1, 1);
+            if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
+              shootRobot(0.0f);
+              setMotorValue(6, 1, 0);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //}}}
+        }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")){
+              driveRobot(-1*(SmartDashboard::GetNumber("AutoSpeed")),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;
+            }
+          }
+          //}}}
+          //Release Ball{{{
+          if(currentStep==1){
+            setMotorValue(6, 1, 102);
+            if(c==50){
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Shoot{{{
+          if(currentStep==2&&c>SmartDashboard::GetNumber("First Shot Start")*200){
+            if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(power);
+            }else{
+              shootRobot(0.0f);
+            }
+            if(c==SmartDashboard::GetNumber("First Shot Stop")*200){
+              shootRobot(0.0f);
+              setMotorValue(6, 1, 0);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Lower Shooter{{{
+          if(currentStep==3){
+            if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(-0.3f);
+            }else{
+              shootRobot(0.0f);
+            }
+            if(c==1*200){
+              shootRobot(0.0f);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Collect Ball{{{
+          if(currentStep==4&&c>SmartDashboard::GetNumber("Second Shot Start")){
+            setMotorValue(6, 1, 1);
+            if(c==SmartDashboard::GetNumber("Second Shot Stop")*200){
+              setMotorValue(6, 1, 0);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //Shoot{{{
+          if(currentStep==5&&c>(SmartDashboard::GetNumber("Second Shot Start"))*200){
+            if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
+              shootRobot(power);
+            }else{
+              shootRobot(0.0f);
+            }
+            setMotorValue(6, 1, 1);
+            if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
+              setMotorValue(6, 1, 0);
+              shootRobot(0.0f);
+              currentStep++;
+              c=0;
+            }
+          }
+          //}}}
+          //}}}
         }
-        //}}}
-        //Shoot{{{
-        if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
-          if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(power);
-          }else{
-            shootRobot(0.0f);
-          }
-          setMotorValue(6, 1, 1);
-          if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
-            shootRobot(0.0f);
-            setMotorValue(6, 1, 0);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Lower Shooter{{{
-        if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
-          if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(-0.3f);
-          }else{
-            shootRobot(0.0f);
-          }
-          if(c==(3.0f)*200){
-            shootRobot(0.0f);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //}}}
-      }else if(SmartDashboard::GetNumber("Autonomous sequence")==1){
-        //Autonomous1{{{
-        //Drive{{{
-        if(currentStep==0){
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
-            driveRobot(-1.0f,correction);
-          }else{
-            driveRobot(0.0f,0.0f);
-          }
-          if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
-            driveRobot(0.0f,0.0f);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Shoot{{{
-        if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
-          if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(power);
-          }else{
-            shootRobot(0.0f);
-          }
-          setMotorValue(6, 1, 1);
-          if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
-            shootRobot(0.0f);
-            setMotorValue(6, 1, 0);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Drive Backwards{{{
-        if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
-          driveRobot(1.0f,correction);
-          if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(-0.2f);
-          }else{
-            shootRobot(0.0f);
-          }
-          if(c==(SmartDashboard::GetNumber("Reverse direction stop"))*200){
-            driveRobot(0.0f,0.0f);
-            shootRobot(0.0f);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Drive{{{
-        if(currentStep==3&&c>SmartDashboard::GetNumber("Second Drive Start")*200){
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
-            driveRobot(-1.0f,correction);
-          }else{
-            driveRobot(0.0f,0.0f);
-          }
-          if(c==(SmartDashboard::GetNumber("Inital Drive Timeout"))*200){
-            driveRobot(0.0f,0.0f);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Shoot{{{
-        if(currentStep==4&&c>(SmartDashboard::GetNumber("Second Shot Start"))*200){
-          if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(power);
-          }else{
-            shootRobot(0.0f);
-          }
-          setMotorValue(6, 1, 1);
-          if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
-            shootRobot(0.0f);
-            setMotorValue(6, 1, 0);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //}}}
-      }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")){
-            driveRobot(-1*(SmartDashboard::GetNumber("AutoSpeed")),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;
-          }
-        }
-        //}}}
-        //Release Ball{{{
-        if(currentStep==1){
-          setMotorValue(6, 1, 102);
-          if(c==50){
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Shoot{{{
-        if(currentStep==2&&c>SmartDashboard::GetNumber("First Shot Start")*200){
-          if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(power);
-          }else{
-            shootRobot(0.0f);
-          }
-          if(c==SmartDashboard::GetNumber("First Shot Stop")*200){
-            shootRobot(0.0f);
-            setMotorValue(6, 1, 0);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Lower Shooter{{{
-        if(currentStep==3){
-          if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(-0.3f);
-          }else{
-            shootRobot(0.0f);
-          }
-          if(c==1*200){
-            shootRobot(0.0f);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Collect Ball{{{
-        if(currentStep==4&&c>SmartDashboard::GetNumber("Second Shot Start")){
-          setMotorValue(6, 1, 1);
-          if(c==SmartDashboard::GetNumber("Second Shot Stop")*200){
-            setMotorValue(6, 1, 0);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //Shoot{{{
-        if(currentStep==5&&c>(SmartDashboard::GetNumber("Second Shot Start"))*200){
-          if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
-            shootRobot(power);
-          }else{
-            shootRobot(0.0f);
-          }
-          setMotorValue(6, 1, 1);
-          if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
-            setMotorValue(6, 1, 0);
-            shootRobot(0.0f);
-            currentStep++;
-            c=0;
-          }
-        }
-        //}}}
-        //}}}
       }
       SmartDashboard::PutNumber("Autonomous step", currentStep);
       updateDashboard();
@@ -511,7 +512,6 @@ public:
       i++;
       c++;
       Wait(0.005f);
-    }
     compressing=false;
     compressor.Stop();
   }