team2059/zaphod
2
0
Fork 0
mirror of https://github.com/team2059/Zaphod synced 2024-12-18 20:12:28 -05:00
Code Issues Releases Wiki Activity

Merge branch 'AutoJoystick'

Browse Source
This commit is contained in:
Austen Adler 2014-03-13 18:32:30 -07:00
commit bf8c4b7e37
1 changed files with 92 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
MyRobot.cpp
View File

@ -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)){
@ -585,6 +587,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);