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);