From 2385e64c1b9649995026446c07976492bb181b29 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Thu, 13 Mar 2014 18:08:11 -0700
Subject: [PATCH] Started to work on joystick

---
 MyRobot.cpp | 93 ++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 92 insertions(+), 1 deletion(-)

diff --git a/MyRobot.cpp b/MyRobot.cpp
index ccfb0a6..92d0994 100644
--- a/MyRobot.cpp
+++ b/MyRobot.cpp
@@ -510,10 +510,12 @@ public:
     collectorSole2.Set(false);
     compressing = false;
     SmartDashboard::PutBoolean("CollectorState",false);
+    int c=0;
+    int currentStep=0;
     //}}}
     while(IsEnabled() && IsOperatorControl()) {
       //Joystick{{{
-      //Shooter{{{
+      //Throttle values{{{
       if(Lstick.GetRawButton(9)==1){
         throttle = (-Lstick.GetRawAxis(4)+1)/2;
       }else if(Lstick.GetRawButton(10)){
@@ -581,6 +583,95 @@ public:
         collectorSole2.Set(false);
         //}}}
       }
+      if(Rstick.GetRawButton(2)==1){
+        c++;
+        //Autonomous From Joystick{{{
+        //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.0f,SmartDashboard::GetNumber("AutoCorrection"));
+          }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(SmartDashboard::GetNumber("AutoPower"));
+          }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==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(SmartDashboard::GetNumber("AutoPower"));
+          }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;
+          }
+        }
+        //}}}
+        //}}}
+      }else{
+        c=0;
+        currentStep=0;
+      }
       //Collector Motor{{{
       if(Lstick.GetRawButton(11)==1) {
         setMotorValue(6, 1, 1);