team2059/zaphod
2
0
Fork 0
mirror of https://github.com/team2059/Zaphod synced 2025-01-07 22:14:14 -05:00
Code Issues Releases Wiki Activity

Fixed auto for match2

Browse Source
This commit is contained in:
Austen Adler 2014-02-28 16:02:53 +00:00
parent 9c9b7904e4
commit 2787077943
1 changed files with 22 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
MyRobot.cpp
View File

@ -107,10 +107,11 @@ public:
SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true)); SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true));
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage())); SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage())); SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
SmartDashboard::PutNumber("AutoDistance",0.0f); SmartDashboard::PutNumber("AutoDistance",50.0f);
SmartDashboard::PutNumber("AutoYValue",50.0f); SmartDashboard::PutNumber("AutoYValue",150.0f);
SmartDashboard::PutNumber("AutoPower",0.52f); SmartDashboard::PutNumber("AutoPower",0.57f);
SmartDashboard::PutNumber("AutoAngle",120.0f); SmartDashboard::PutNumber("AutoAngle",130.0f);
SmartDashboard::PutNumber("AutoCorrection",0.1f);
SmartDashboard::PutBoolean("Use Ultrasonic",false); SmartDashboard::PutBoolean("Use Ultrasonic",false);
SmartDashboard::PutBoolean("Daniel Mode",false); SmartDashboard::PutBoolean("Daniel Mode",false);
} }
@ -352,12 +353,15 @@ public:
}else{ }else{
int x=SmartDashboard::GetNumber("AutoDistance"); int x=SmartDashboard::GetNumber("AutoDistance");
int y=SmartDashboard::GetNumber("AutoYValue"); int y=SmartDashboard::GetNumber("AutoYValue");
int power=SmartDashboard::GetNumber("AutoPower"); float power=SmartDashboard::GetNumber("AutoPower");
int angle=SmartDashboard::GetNumber("AutoAngle"); int angle=SmartDashboard::GetNumber("AutoAngle");
setMotorValue(6, 1, 1); float correction=SmartDashboard::GetNumber("AutoCorrection");
if(i<1700+3*x+2*y){
setMotorValue(6, 1, 1);
}
if(i<200+x) { if(i<200+x) {
//Forward .5s //Forward .5s
driveRobot(-1.0f,0.1f); driveRobot(-1.0f,correction);
shootRobot(0.0f, true); shootRobot(0.0f, true);
}else if(i>=200+x&&i<=400+x){ }else if(i>=200+x&&i<=400+x){
//Wait //Wait
@ -374,35 +378,35 @@ public:
} else if(i>500+x&&i<700+2*x+y) { } else if(i>500+x&&i<700+2*x+y) {
//Drive backward 1s, Collect ball //Drive backward 1s, Collect ball
if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){ if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(-0.15f); shootRobot(-0.30f,true);
} }
driveRobot(1.0f,0.1f); driveRobot(0.6f,correction);
shootRobot(0.0f,true); shootRobot(0.0f,true);
}else if(i>=700+2*x+y&&i<=900+2*x+y){ } else if(i>=700+2*x+y&&i<=1300+2*x+y){
//Wait //Wait
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
shootRobot(0.0f,true); shootRobot(0.0f,true);
} else if(i>900+2*x+y&&i<1100+3*x+2*y) { } else if(i>1300+2*x+y&&i<1500+3*x+2*y) {
//Drive forward 1s //Drive forward 1s
driveRobot(-1.0f,0.1f); driveRobot(-1.0f,correction);
shootRobot(0.0f,true); shootRobot(0.0f,true);
} else if(i>=1100+3*x+2*y&&i<=1200+3*x+2*y){ } else if(i>=1500+3*x+2*y&&i<=1600+3*x+2*y){
//Wait //Wait
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
shootRobot(0.0f,true); shootRobot(0.0f,true);
} else if(i>1200+3*x+2*y&&i<1300+3*x+2*y&&/*120*/angle>=potToDegrees(armPot.GetAverageVoltage())){ } else if(i>1600+3*x+2*y&&i<1700+3*x+2*y&&/*120*/angle>=potToDegrees(armPot.GetAverageVoltage())){
//Shoot //Shoot
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
shootRobot(power,true); shootRobot(power,true);
} else if(i>1200+3*x+2*y&&i<1300+3*x+2*y&&/*120*/angle<=potToDegrees(armPot.GetAverageVoltage())){ } else if(i>1600+3*x+2*y&&i<1700+3*x+2*y&&/*120*/angle<=potToDegrees(armPot.GetAverageVoltage())){
//Wait //Wait
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
shootRobot(0.0f,true); shootRobot(0.0f,true);
} else if(i>1300+3*x+2*y&&40.0f>=potToDegrees(armPot.GetAverageVoltage())) { } else if(i>1700+3*x+2*y&&40.0f<=potToDegrees(armPot.GetAverageVoltage())) {
//Stop robot after auto, let down shooter //Stop robot after auto, let down shooter
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
shootRobot(-0.15f,true); shootRobot(-0.15,true);
} else if(i>1300+3*x+2*y&&40.0f<=potToDegrees(armPot.GetAverageVoltage())) { } else if(i>1700+3*x+2*y&&40.0f>=potToDegrees(armPot.GetAverageVoltage())) {
//Stop all motors //Stop all motors
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
shootRobot(0.0f,true); shootRobot(0.0f,true);