mirror of
https://github.com/team2059/Zaphod
synced 2024-12-18 20:12:28 -05:00
Added a half power switch that disables auto and moves robot at half power
This commit is contained in:
parent
13a1d2d6c6
commit
c1babe139a
427
MyRobot.cpp
427
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")){
|
||||
//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(!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;
|
||||
}
|
||||
}
|
||||
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;
|
||||
//}}}
|
||||
}
|
||||
//}}}
|
||||
//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();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user