Added a half power switch that disables auto and moves robot at half power

MyRobot.cpp

@@ -104,6 +104,7 @@ public:
     SmartDashboard::PutNumber("ShooterButtonPower7",1.0f);
     SmartDashboard::PutNumber("ShooterButtonPower8",0.5f);
     //Bool switches
+    SmartDashboard::PutBoolean("HalfPower",false);
     SmartDashboard::PutBoolean("OneBallAuto",false);
     SmartDashboard::PutBoolean("Use Ultrasonic",true);
     SmartDashboard::PutBoolean("Daniel Mode",false);
@@ -147,6 +148,10 @@ public:
     }
     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);
@@ -263,219 +268,221 @@ public:
     SmartDashboard::PutBoolean("CollectorState",true);
     //}}}
     while(IsEnabled()&&IsAutonomous()){
-      switch (SmartDashboard::GetNumber("Autonomous Sequence")){
+      if(!SmartDashboard::GetBoolean("HalfPower")){
-        //Autonomous0{{{
+        switch (SmartDashboard::GetNumber("Autonomous Sequence")){
-      case 0:
+          //Autonomous0{{{
-        //Drive{{{
+        case 0:
-        if(currentStep==0){
+          //Drive{{{
-          if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
+          if(currentStep==0){
-            driveRobot(-1.0f,correction);
+            if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
-          }else{
+              driveRobot(-1.0f,correction);
-            driveRobot(0.0f,0.0f);
+            }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);
+          //Shoot{{{
-            currentStep++;
+          if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
-            c=0;
+            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;
+            }
+          }
+          //}}}
+          break;
+          //}}}
+          //Autonomous1{{{
+        case 1:
+          //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;
+            }
+          }
+          //}}}
+          break;
+          //}}}
+          //Autonomous2{{{
+        case 2:
+          //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;
+            }
+          }
+          //}}}
+          break;
+          //}}}
         }
-        //}}}
-        //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;
-          }
-        }
-        //}}}
-        break;
-        //}}}
-        //Autonomous1{{{
-      case 1:
-        //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;
-          }
-        }
-        //}}}
-        break;
-        //}}}
-        //Autonomous2{{{
-      case 2:
-        //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;
-          }
-        }
-        //}}}
-        break;
-        //}}}
       }
       SmartDashboard::PutNumber("Autonomous step",currentStep);
       updateDashboard();