mirror of
https://github.com/team2059/Zaphod
synced 2024-12-18 20:12:28 -05:00
Started to work on joystick
This commit is contained in:
parent
ba8df23159
commit
2385e64c1b
93
MyRobot.cpp
93
MyRobot.cpp
@ -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)){
|
||||
@ -581,6 +583,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);
|
||||
|
Loading…
Reference in New Issue
Block a user