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

Added auto in teleop feature (Rstick button 8)

Browse Source
This commit is contained in:
Austen Adler 2014-10-25 14:59:56 -04:00
parent fdf83700dd
commit 44fb8ac9c9
1 changed files with 181 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
src/Zaphod.cpp
View File

@ -613,9 +613,122 @@ public:
collectorSole2.Set(false);
compressing=false;
SmartDashboard::PutBoolean("CollectorState",false);
bool runningAuto=false;
int c;
float power;
float correction;
int currentStep;
//}}}
while(IsEnabled()&&IsOperatorControl()){
//Joystick{{{
//Auto in teleop{{{
if(!runningAuto&&Rstick.GetRawButton(8)==1){
printf("Initializing auto in teleop\n");
runningAuto=true;
c=0;
collectorSole1.Set(false);
collectorSole2.Set(true);
WallLeft.Set(1);
BallLeft.Set(0);
WallRight.Set(1);
BallRight.Set(0);
SmartDashboard::PutBoolean("CollectorState",true);
currentStep=0;
power=SmartDashboard::GetNumber("AutoPower");
correction=SmartDashboard::GetNumber("AutoCorrection");
}
if(Rstick.GetRawButton(8)!=1){
runningAuto=false;
}
if(runningAuto){
//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.1f);
}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;
}
}
//}}}
if(currentStep==6){
runningAuto=false;
}
c++;
//}}}
//Standard Teleop{{{
}else{
//Throttle values{{{
if(Lstick.GetRawButton(9)==1){
throttle=(-Lstick.GetRawAxis(4)+1)/2;
@ -675,11 +788,7 @@ public:
setMotorValue(6,1,0);
}
//}}}
//}}}
//Driving{{{
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
//printf("GetY: %f, GetX/Z: %f\n", Rstick.GetY()),((Rstick.GetZ()+Rstick.GetX()));
//}}}
if(shooter_idle==false){ //Start the shooting sequence by telling the robot the arm needs to complete a shot (and isn't finished yet)
if(shooter_fired(2)==false){ //Only do this if the shooter hasn't "fired" (in the upwards state)
shooter_fire(shooter_clear_to_fire(), throttle, upLimit, potToDegrees(armPot.GetAverageVoltage()));
@ -693,6 +802,8 @@ public:
//Stop the shooter if it should be idle
shooter_stop();
}
}
//}}}
runCompressor(i,100);
updateDashboard();
i++;