mirror of
https://github.com/team2059/Zaphod
synced 2024-12-18 20:12:28 -05:00
Added auto in teleop feature (Rstick button 8)
This commit is contained in:
parent
fdf83700dd
commit
44fb8ac9c9
251
src/Zaphod.cpp
251
src/Zaphod.cpp
@ -613,86 +613,197 @@ public:
|
|||||||
collectorSole2.Set(false);
|
collectorSole2.Set(false);
|
||||||
compressing=false;
|
compressing=false;
|
||||||
SmartDashboard::PutBoolean("CollectorState",false);
|
SmartDashboard::PutBoolean("CollectorState",false);
|
||||||
|
bool runningAuto=false;
|
||||||
|
int c;
|
||||||
|
float power;
|
||||||
|
float correction;
|
||||||
|
int currentStep;
|
||||||
//}}}
|
//}}}
|
||||||
while(IsEnabled()&&IsOperatorControl()){
|
while(IsEnabled()&&IsOperatorControl()){
|
||||||
//Joystick{{{
|
//Joystick{{{
|
||||||
//Throttle values{{{
|
//Auto in teleop{{{
|
||||||
if(Lstick.GetRawButton(9)==1){
|
if(!runningAuto&&Rstick.GetRawButton(8)==1){
|
||||||
throttle=(-Lstick.GetRawAxis(4)+1)/2;
|
printf("Initializing auto in teleop\n");
|
||||||
}else if(Lstick.GetRawButton(10)){
|
runningAuto=true;
|
||||||
throttle=SmartDashboard::GetNumber("ShooterButtonPower10");
|
c=0;
|
||||||
}else if(Lstick.GetRawButton(7)){
|
|
||||||
throttle=SmartDashboard::GetNumber("ShooterButtonPower7");
|
|
||||||
}else if(Lstick.GetRawButton(8)){
|
|
||||||
throttle=SmartDashboard::GetNumber("ShooterButtonPower8");
|
|
||||||
}
|
|
||||||
//}}}
|
|
||||||
//Shooter upLimit{{{
|
|
||||||
if(Lstick.GetRawButton(3)){
|
|
||||||
upLimit=100.0f;
|
|
||||||
}
|
|
||||||
if(Lstick.GetRawButton(4)){
|
|
||||||
upLimit=120.0f;
|
|
||||||
}
|
|
||||||
if(Lstick.GetRawButton(5)){
|
|
||||||
upLimit=90.0f;
|
|
||||||
}
|
|
||||||
if(Lstick.GetRawButton(6)){
|
|
||||||
upLimit=130.0f;
|
|
||||||
}
|
|
||||||
//}}}
|
|
||||||
//Shoot{{{
|
|
||||||
if(Lstick.GetRawButton(1)==1){
|
|
||||||
compressorEnabled=false;
|
|
||||||
SmartDashboard::PutBoolean("Compressor Enabled",false);
|
|
||||||
if(compressing){
|
|
||||||
printf("Forcing compressor off\n");
|
|
||||||
compressor.Stop();
|
|
||||||
}
|
|
||||||
compressing=false;
|
|
||||||
shooter_idle=false; //Cause the robot to start the shooting sequence
|
|
||||||
}
|
|
||||||
//}}}
|
|
||||||
//Collector Arm{{{
|
|
||||||
if(Rstick.GetRawButton(9)==1){
|
|
||||||
SmartDashboard::PutBoolean("CollectorState",true);
|
|
||||||
collectorExtended=true;
|
|
||||||
collectorSole1.Set(false);
|
collectorSole1.Set(false);
|
||||||
collectorSole2.Set(true);
|
collectorSole2.Set(true);
|
||||||
}else if(Rstick.GetRawButton(10)==1){
|
WallLeft.Set(1);
|
||||||
SmartDashboard::PutBoolean("CollectorState",false);
|
BallLeft.Set(0);
|
||||||
collectorExtended=false;
|
WallRight.Set(1);
|
||||||
collectorSole1.Set(true);
|
BallRight.Set(0);
|
||||||
collectorSole2.Set(false);
|
SmartDashboard::PutBoolean("CollectorState",true);
|
||||||
|
currentStep=0;
|
||||||
|
power=SmartDashboard::GetNumber("AutoPower");
|
||||||
|
correction=SmartDashboard::GetNumber("AutoCorrection");
|
||||||
|
|
||||||
}
|
}
|
||||||
//}}}
|
if(Rstick.GetRawButton(8)!=1){
|
||||||
//Collector Motor{{{
|
runningAuto=false;
|
||||||
if(Rstick.GetRawButton(1)==1&&50>=(potToDegrees(armPot.GetAverageVoltage()))){
|
|
||||||
setMotorValue(6,1,1);
|
|
||||||
}else if(Rstick.GetRawButton(2)==1){
|
|
||||||
setMotorValue(6,1,255);
|
|
||||||
}else if(shooter_idle==true){
|
|
||||||
setMotorValue(6,1,0);
|
|
||||||
}
|
}
|
||||||
//}}}
|
if(runningAuto){
|
||||||
//}}}
|
//Drive{{{
|
||||||
//Driving{{{
|
if(currentStep==0&&c>SmartDashboard::GetNumber("Initial Drive Delay")*200){
|
||||||
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
setMotorValue(6, 1, SmartDashboard::GetNumber("Collector Speed"));
|
||||||
//printf("GetY: %f, GetX/Z: %f\n", Rstick.GetY()),((Rstick.GetZ()+Rstick.GetX()));
|
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
||||||
//}}}
|
driveRobot(-1*(SmartDashboard::GetNumber("AutoSpeed")),correction);
|
||||||
if(shooter_idle==false){ //Start the shooting sequence by telling the robot the arm needs to complete a shot (and isn't finished yet)
|
}else{
|
||||||
if(shooter_fired(2)==false){ //Only do this if the shooter hasn't "fired" (in the upwards state)
|
driveRobot(0.0f,0.0f);
|
||||||
shooter_fire(shooter_clear_to_fire(), throttle, upLimit, potToDegrees(armPot.GetAverageVoltage()));
|
}
|
||||||
|
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
|
||||||
|
setMotorValue(6, 1, 0);
|
||||||
|
driveRobot(0.0f,0.0f);
|
||||||
|
currentStep++;
|
||||||
|
c=0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if(shooter_fired(2)==true){
|
//}}}
|
||||||
//After the shooter has fired, lower the shooter arm then reset the whole shooting sequence
|
//Release Ball{{{
|
||||||
shooter_lower(-0.1, 47, potToDegrees(armPot.GetAverageVoltage()));
|
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;
|
||||||
|
}else if(Lstick.GetRawButton(10)){
|
||||||
|
throttle=SmartDashboard::GetNumber("ShooterButtonPower10");
|
||||||
|
}else if(Lstick.GetRawButton(7)){
|
||||||
|
throttle=SmartDashboard::GetNumber("ShooterButtonPower7");
|
||||||
|
}else if(Lstick.GetRawButton(8)){
|
||||||
|
throttle=SmartDashboard::GetNumber("ShooterButtonPower8");
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
//Shooter upLimit{{{
|
||||||
|
if(Lstick.GetRawButton(3)){
|
||||||
|
upLimit=100.0f;
|
||||||
|
}
|
||||||
|
if(Lstick.GetRawButton(4)){
|
||||||
|
upLimit=120.0f;
|
||||||
|
}
|
||||||
|
if(Lstick.GetRawButton(5)){
|
||||||
|
upLimit=90.0f;
|
||||||
|
}
|
||||||
|
if(Lstick.GetRawButton(6)){
|
||||||
|
upLimit=130.0f;
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
//Shoot{{{
|
||||||
|
if(Lstick.GetRawButton(1)==1){
|
||||||
|
compressorEnabled=false;
|
||||||
|
SmartDashboard::PutBoolean("Compressor Enabled",false);
|
||||||
|
if(compressing){
|
||||||
|
printf("Forcing compressor off\n");
|
||||||
|
compressor.Stop();
|
||||||
|
}
|
||||||
|
compressing=false;
|
||||||
|
shooter_idle=false; //Cause the robot to start the shooting sequence
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
//Collector Arm{{{
|
||||||
|
if(Rstick.GetRawButton(9)==1){
|
||||||
|
SmartDashboard::PutBoolean("CollectorState",true);
|
||||||
|
collectorExtended=true;
|
||||||
|
collectorSole1.Set(false);
|
||||||
|
collectorSole2.Set(true);
|
||||||
|
}else if(Rstick.GetRawButton(10)==1){
|
||||||
|
SmartDashboard::PutBoolean("CollectorState",false);
|
||||||
|
collectorExtended=false;
|
||||||
|
collectorSole1.Set(true);
|
||||||
|
collectorSole2.Set(false);
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
//Collector Motor{{{
|
||||||
|
if(Rstick.GetRawButton(1)==1&&50>=(potToDegrees(armPot.GetAverageVoltage()))){
|
||||||
|
setMotorValue(6,1,1);
|
||||||
|
}else if(Rstick.GetRawButton(2)==1){
|
||||||
|
setMotorValue(6,1,255);
|
||||||
|
}else if(shooter_idle==true){
|
||||||
|
setMotorValue(6,1,0);
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
driveRobot(-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()));
|
||||||
|
}
|
||||||
|
if(shooter_fired(2)==true){
|
||||||
|
//After the shooter has fired, lower the shooter arm then reset the whole shooting sequence
|
||||||
|
shooter_lower(-0.1, 47, potToDegrees(armPot.GetAverageVoltage()));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(shooter_idle==true){
|
||||||
|
//Stop the shooter if it should be idle
|
||||||
|
shooter_stop();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if(shooter_idle==true){
|
//}}}
|
||||||
//Stop the shooter if it should be idle
|
|
||||||
shooter_stop();
|
|
||||||
}
|
|
||||||
runCompressor(i,100);
|
runCompressor(i,100);
|
||||||
updateDashboard();
|
updateDashboard();
|
||||||
i++;
|
i++;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user