diff --git a/MyRobot.cpp b/MyRobot.cpp index a076866..6e60aa8 100644 --- a/MyRobot.cpp +++ b/MyRobot.cpp @@ -269,223 +269,6 @@ public: //}}} while(IsEnabled()&&IsAutonomous()){ if(!SmartDashboard::GetBoolean("HalfPower")){ - switch (SmartDashboard::GetNumber("Autonomous Sequence")){ - //Autonomous0{{{ - case 0: - //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; - } - } - //}}} - //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(); //Compressor{{{ if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1){ compressor.Stop();