mirror of
https://github.com/team2059/Zaphod
synced 2024-12-18 20:12:28 -05:00
Formatted code
This commit is contained in:
parent
4fe2261a99
commit
13a1d2d6c6
506
MyRobot.cpp
506
MyRobot.cpp
@ -9,14 +9,14 @@ class RobotDemo : public SimpleRobot
|
|||||||
{
|
{
|
||||||
//Declarations{{{
|
//Declarations{{{
|
||||||
RobotDrive myRobot;
|
RobotDrive myRobot;
|
||||||
bool collectorExtended, shooting, compressing, allowCompressing;
|
bool collectorExtended,shooting,compressing,allowCompressing;
|
||||||
float upLimit, throttle;
|
float upLimit,throttle;
|
||||||
Joystick Rstick, Lstick;
|
Joystick Rstick,Lstick;
|
||||||
Solenoid collectorSole1, collectorSole2;
|
Solenoid collectorSole1,collectorSole2;
|
||||||
Compressor compressor;
|
Compressor compressor;
|
||||||
Jaguar Left1, Left2, Left3, Right1, Right2, Right3, RightArmMotor1, RightArmMotor2, LeftArmMotor1, LeftArmMotor2, CollectorMotor1;
|
Jaguar Left1,Left2,Left3,Right1,Right2,Right3,RightArmMotor1,RightArmMotor2,LeftArmMotor1,LeftArmMotor2,CollectorMotor1;
|
||||||
AnalogChannel armPot, BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
|
AnalogChannel armPot,BallSonicLeft,BallSonicRight,WallSonicLeft,WallSonicRight;
|
||||||
DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
|
DigitalOutput BallLeft,BallRight,WallLeft,WallRight;
|
||||||
//}}}
|
//}}}
|
||||||
public:
|
public:
|
||||||
RobotDemo():
|
RobotDemo():
|
||||||
@ -36,7 +36,7 @@ public:
|
|||||||
BallRight(2,4),
|
BallRight(2,4),
|
||||||
WallRight(2,5),
|
WallRight(2,5),
|
||||||
//Compressor
|
//Compressor
|
||||||
compressor(2, 5, 1, 1),
|
compressor(2,5,1,1),
|
||||||
//Solenoids
|
//Solenoids
|
||||||
collectorSole1(1),
|
collectorSole1(1),
|
||||||
collectorSole2(2),
|
collectorSole2(2),
|
||||||
@ -48,38 +48,38 @@ public:
|
|||||||
Right2(2,2),
|
Right2(2,2),
|
||||||
Right3(2,3),
|
Right3(2,3),
|
||||||
//Shooter Motors
|
//Shooter Motors
|
||||||
LeftArmMotor1(1, 4),
|
LeftArmMotor1(1,4),
|
||||||
LeftArmMotor2(1, 5),
|
LeftArmMotor2(1,5),
|
||||||
RightArmMotor1(2, 4),
|
RightArmMotor1(2,4),
|
||||||
RightArmMotor2(2, 5),
|
RightArmMotor2(2,5),
|
||||||
//Collector Motor
|
//Collector Motor
|
||||||
CollectorMotor1(1, 6),
|
CollectorMotor1(1,6),
|
||||||
myRobot(Left1, Left2, Right1, Right2) {
|
myRobot(Left1,Left2,Right1,Right2){
|
||||||
GetWatchdog().SetEnabled(false);
|
GetWatchdog().SetEnabled(false);
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//RobotInit{{{
|
//RobotInit{{{
|
||||||
void RobotInit() {
|
void RobotInit(){
|
||||||
DashboardSetup();
|
DashboardSetup();
|
||||||
upLimit = 130.0;
|
upLimit=130.0;
|
||||||
compressor.Start();
|
compressor.Start();
|
||||||
shooting = false;
|
shooting=false;
|
||||||
compressing = true;
|
compressing=true;
|
||||||
allowCompressing = true;
|
allowCompressing=true;
|
||||||
throttle=(-Lstick.GetRawAxis(4)+1)/2;
|
throttle=(-Lstick.GetRawAxis(4)+1)/2;
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//DashboardSetup{{{
|
//DashboardSetup{{{
|
||||||
void DashboardSetup() {
|
void DashboardSetup(){
|
||||||
SmartDashboard::PutNumber("Throttle", throttle);
|
SmartDashboard::PutNumber("Throttle",throttle);
|
||||||
SmartDashboard::PutNumber("upLimit", 120.0f);
|
SmartDashboard::PutNumber("upLimit",120.0f);
|
||||||
SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
|
SmartDashboard::PutNumber("armPot",potToDegrees(armPot.GetAverageVoltage()));
|
||||||
SmartDashboard::PutNumber("Log Level", 1.0f);
|
SmartDashboard::PutNumber("Log Level",1.0f);
|
||||||
//Ultrasonic
|
//Ultrasonic
|
||||||
SmartDashboard::PutNumber("Wall Left", voltToDistance(WallSonicLeft.GetAverageVoltage(),true));
|
SmartDashboard::PutNumber("Wall Left",voltToDistance(WallSonicLeft.GetAverageVoltage(),true));
|
||||||
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()));
|
||||||
//Autonomous values
|
//Autonomous values
|
||||||
SmartDashboard::PutNumber("AutoSpeed",0.95f);
|
SmartDashboard::PutNumber("AutoSpeed",0.95f);
|
||||||
SmartDashboard::PutNumber("Auto Distance",65.0f);
|
SmartDashboard::PutNumber("Auto Distance",65.0f);
|
||||||
@ -87,17 +87,17 @@ public:
|
|||||||
SmartDashboard::PutNumber("AutoPower",0.46f);
|
SmartDashboard::PutNumber("AutoPower",0.46f);
|
||||||
SmartDashboard::PutNumber("AutoCorrection",0.032f);
|
SmartDashboard::PutNumber("AutoCorrection",0.032f);
|
||||||
SmartDashboard::PutNumber("Initial Drive Delay",2.0f);
|
SmartDashboard::PutNumber("Initial Drive Delay",2.0f);
|
||||||
SmartDashboard::PutNumber("Inital Drive Timeout", 4.5f);
|
SmartDashboard::PutNumber("Inital Drive Timeout",4.5f);
|
||||||
SmartDashboard::PutNumber("First Shot Start", 0.5f);
|
SmartDashboard::PutNumber("First Shot Start",0.5f);
|
||||||
SmartDashboard::PutNumber("First Shot Stop", 1.0f);
|
SmartDashboard::PutNumber("First Shot Stop",1.0f);
|
||||||
SmartDashboard::PutNumber("Reverse direction start",0.0f);
|
SmartDashboard::PutNumber("Reverse direction start",0.0f);
|
||||||
SmartDashboard::PutNumber("Reverse direction stop",2.5f);
|
SmartDashboard::PutNumber("Reverse direction stop",2.5f);
|
||||||
SmartDashboard::PutNumber("Second Drive Start", 1.0f);
|
SmartDashboard::PutNumber("Second Drive Start",1.0f);
|
||||||
SmartDashboard::PutNumber("Second Drive Timeout", 2.5f);
|
SmartDashboard::PutNumber("Second Drive Timeout",2.5f);
|
||||||
SmartDashboard::PutNumber("Second Shot Start", 0.5f);
|
SmartDashboard::PutNumber("Second Shot Start",0.5f);
|
||||||
SmartDashboard::PutNumber("Second Shot Stop", 1.0f);
|
SmartDashboard::PutNumber("Second Shot Stop",1.0f);
|
||||||
SmartDashboard::PutNumber("Autonomous step",0.0f);
|
SmartDashboard::PutNumber("Autonomous step",0.0f);
|
||||||
SmartDashboard::PutNumber("Autonomous sequence", 2.0f);
|
SmartDashboard::PutNumber("Autonomous sequence",2.0f);
|
||||||
//Shooter presets
|
//Shooter presets
|
||||||
SmartDashboard::PutNumber("ShortRange",0.465f); //Power for the shooter when against the low goal
|
SmartDashboard::PutNumber("ShortRange",0.465f); //Power for the shooter when against the low goal
|
||||||
SmartDashboard::PutNumber("ShooterButtonPower10",0.605f);
|
SmartDashboard::PutNumber("ShooterButtonPower10",0.605f);
|
||||||
@ -108,144 +108,144 @@ public:
|
|||||||
SmartDashboard::PutBoolean("Use Ultrasonic",true);
|
SmartDashboard::PutBoolean("Use Ultrasonic",true);
|
||||||
SmartDashboard::PutBoolean("Daniel Mode",false);
|
SmartDashboard::PutBoolean("Daniel Mode",false);
|
||||||
SmartDashboard::PutBoolean("CollectorState",false);
|
SmartDashboard::PutBoolean("CollectorState",false);
|
||||||
SmartDashboard::PutBoolean("Compressor Enabled", allowCompressing);
|
SmartDashboard::PutBoolean("Compressor Enabled",allowCompressing);
|
||||||
SmartDashboard::PutBoolean("Compressor Running", compressing);
|
SmartDashboard::PutBoolean("Compressor Running",compressing);
|
||||||
SmartDashboard::PutBoolean("Ignore Pot",false);
|
SmartDashboard::PutBoolean("Ignore Pot",false);
|
||||||
//Battery voltage
|
//Battery voltage
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//updateDashboard{{{
|
//updateDashboard{{{
|
||||||
void updateDashboard() {
|
void updateDashboard(){
|
||||||
SmartDashboard::PutNumber("Throttle", throttle);
|
SmartDashboard::PutNumber("Throttle",throttle);
|
||||||
SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
|
SmartDashboard::PutNumber("armPot",potToDegrees(armPot.GetAverageVoltage()));
|
||||||
SmartDashboard::PutNumber("Wall Left", voltToDistance(WallSonicLeft.GetAverageVoltage(),true));
|
SmartDashboard::PutNumber("Wall Left",voltToDistance(WallSonicLeft.GetAverageVoltage(),true));
|
||||||
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("upLimit", upLimit);
|
SmartDashboard::PutNumber("upLimit",upLimit);
|
||||||
SmartDashboard::PutBoolean("Compressor Running", compressing);
|
SmartDashboard::PutBoolean("Compressor Running",compressing);
|
||||||
allowCompressing = SmartDashboard::GetBoolean("Compressor Enabled");
|
allowCompressing=SmartDashboard::GetBoolean("Compressor Enabled");
|
||||||
if(upLimit > 167) {
|
if(upLimit > 167){
|
||||||
upLimit = 167;
|
upLimit=167;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//shootRobot{{{
|
//shootRobot{{{
|
||||||
void shootRobot(float power=0) {
|
void shootRobot(float power=0){
|
||||||
setMotorValue(4, 1, cvt(power));
|
setMotorValue(4,1,cvt(power));
|
||||||
setMotorValue(5, 1, cvt(power));
|
setMotorValue(5,1,cvt(power));
|
||||||
setMotorValue(4, 2, cvt(-power));
|
setMotorValue(4,2,cvt(-power));
|
||||||
setMotorValue(5, 2, cvt(-power));
|
setMotorValue(5,2,cvt(-power));
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//driveRobot{{{
|
//driveRobot{{{
|
||||||
void driveRobot(float x, float y) {
|
void driveRobot(float x,float y){
|
||||||
if(y>1.0f) {
|
if(y>1.0f){
|
||||||
y=1.0f;
|
y=1.0f;
|
||||||
} else if(y!=0.0f&&y<-1.0f) {
|
}else if(y!=0.0f&&y<-1.0f){
|
||||||
y=-1.0f;
|
y=-1.0f;
|
||||||
}
|
|
||||||
int leftPower = ((y+x)/2+1)*127+1;
|
|
||||||
int rightPower = ((y-x)/2+1)*127+1;
|
|
||||||
setMotorValue(1, 1, leftPower);
|
|
||||||
setMotorValue(2, 1, leftPower);
|
|
||||||
setMotorValue(3, 1, leftPower);
|
|
||||||
setMotorValue(1, 2, rightPower);
|
|
||||||
setMotorValue(2, 2, rightPower);
|
|
||||||
setMotorValue(3, 2, rightPower);
|
|
||||||
}
|
}
|
||||||
|
int leftPower=((y+x)/2+1)*127+1;
|
||||||
|
int rightPower=((y-x)/2+1)*127+1;
|
||||||
|
setMotorValue(1,1,leftPower);
|
||||||
|
setMotorValue(2,1,leftPower);
|
||||||
|
setMotorValue(3,1,leftPower);
|
||||||
|
setMotorValue(1,2,rightPower);
|
||||||
|
setMotorValue(2,2,rightPower);
|
||||||
|
setMotorValue(3,2,rightPower);
|
||||||
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//voltToDistance{{{
|
//voltToDistance{{{
|
||||||
float voltToDistance(float a,bool wall=false) {
|
float voltToDistance(float a,bool wall=false){
|
||||||
if(wall) {
|
if(wall){
|
||||||
return (a / 0.00488f) / 2.54f;
|
return (a/0.00488f)/2.54f;
|
||||||
} else {
|
}else{
|
||||||
return (a / 0.000976562f) / 25.4f;
|
return (a/0.000976562f)/25.4f;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//potToDegrees{{{
|
//potToDegrees{{{
|
||||||
//TODO: Test this!
|
//TODO: Test this!
|
||||||
float potToDegrees(float a) {
|
float potToDegrees(float a){
|
||||||
float max = -.0003948;
|
float max=-.0003948;
|
||||||
float min = 5.0245547;
|
float min=5.0245547;
|
||||||
return 300-(a*(300/(min-max)));
|
return 300-(a*(300/(min-max)));
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//cvt{{{
|
//cvt{{{
|
||||||
int cvt(float input) {
|
int cvt(float input){
|
||||||
return input * 127.0f + 128;
|
return input * 127.0f + 128;
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//setMotorValue{{{
|
//setMotorValue{{{
|
||||||
void setMotorValue(int motor, int subwayStation = 1, int value = 127) {
|
void setMotorValue(int motor,int subwayStation=1,int value=127){
|
||||||
if(subwayStation == 1) {
|
if(subwayStation == 1){
|
||||||
//subwayStation1{{{
|
//subwayStation1{{{
|
||||||
switch(motor) {
|
switch(motor){
|
||||||
case 1:
|
case 1:
|
||||||
Left1.SetRaw(value);
|
Left1.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
Left2.SetRaw(value);
|
Left2.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
Left3.SetRaw(value);
|
Left3.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
LeftArmMotor1.SetRaw(value);
|
LeftArmMotor1.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
LeftArmMotor2.SetRaw(value);
|
LeftArmMotor2.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 6:
|
case 6:
|
||||||
CollectorMotor1.SetRaw(value);
|
CollectorMotor1.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 7:
|
case 7:
|
||||||
break;
|
break;
|
||||||
case 8:
|
case 8:
|
||||||
break;
|
break;
|
||||||
case 9:
|
case 9:
|
||||||
break;
|
break;
|
||||||
case 10:
|
case 10:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
} else if(subwayStation == 2) {
|
}else if(subwayStation == 2){
|
||||||
//subwayStation2{{{
|
//subwayStation2{{{
|
||||||
switch(motor) {
|
switch(motor){
|
||||||
//Shooter motors
|
//Shooter motors
|
||||||
case 1:
|
case 1:
|
||||||
Right1.SetRaw(value);
|
Right1.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
Right2.SetRaw(value);
|
Right2.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
Right3.SetRaw(value);
|
Right3.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
RightArmMotor1.SetRaw(value);
|
RightArmMotor1.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
RightArmMotor2.SetRaw(value);
|
RightArmMotor2.SetRaw(value);
|
||||||
break;
|
break;
|
||||||
case 6:
|
case 6:
|
||||||
break;
|
break;
|
||||||
case 7:
|
case 7:
|
||||||
break;
|
break;
|
||||||
case 8:
|
case 8:
|
||||||
break;
|
break;
|
||||||
case 9:
|
case 9:
|
||||||
break;
|
break;
|
||||||
case 10:
|
case 10:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//Autonomous{{{
|
//Autonomous{{{
|
||||||
void Autonomous() {
|
void Autonomous(){
|
||||||
//Initializations{{{
|
//Initializations{{{
|
||||||
myRobot.SetSafetyEnabled(false);
|
myRobot.SetSafetyEnabled(false);
|
||||||
int i=0;
|
int i=0;
|
||||||
@ -262,58 +262,10 @@ public:
|
|||||||
BallRight.Set(0);
|
BallRight.Set(0);
|
||||||
SmartDashboard::PutBoolean("CollectorState",true);
|
SmartDashboard::PutBoolean("CollectorState",true);
|
||||||
//}}}
|
//}}}
|
||||||
while(IsEnabled()&&IsAutonomous()) {
|
while(IsEnabled()&&IsAutonomous()){
|
||||||
switch (SmartDashboard::GetNumber("Autonomous Sequence")){
|
switch (SmartDashboard::GetNumber("Autonomous Sequence")){
|
||||||
//Autonomous0{{{
|
//Autonomous0{{{
|
||||||
case 0:
|
case 0:
|
||||||
//Drive{{{
|
|
||||||
if(currentStep==0){
|
|
||||||
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
|
||||||
driveRobot(-1.0f,correction);
|
|
||||||
}else{
|
|
||||||
driveRobot(0.0f,0.0f);
|
|
||||||
}
|
|
||||||
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
|
|
||||||
driveRobot(0.0f,0.0f);
|
|
||||||
currentStep++;
|
|
||||||
c=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
//}}}
|
|
||||||
//Shoot{{{
|
|
||||||
if(currentStep==1&&c>SmartDashboard::GetNumber("First 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){
|
|
||||||
shootRobot(0.0f);
|
|
||||||
setMotorValue(6, 1, 0);
|
|
||||||
currentStep++;
|
|
||||||
c=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
//}}}
|
|
||||||
//Lower Shooter{{{
|
|
||||||
if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
|
|
||||||
if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
|
|
||||||
shootRobot(-0.3f);
|
|
||||||
}else{
|
|
||||||
shootRobot(0.0f);
|
|
||||||
}
|
|
||||||
if(c==(3.0f)*200){
|
|
||||||
shootRobot(0.0f);
|
|
||||||
currentStep++;
|
|
||||||
c=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
//}}}
|
|
||||||
break;
|
|
||||||
//}}}
|
|
||||||
//Autonomous1{{{
|
|
||||||
case 1:
|
|
||||||
//Drive{{{
|
//Drive{{{
|
||||||
if(currentStep==0){
|
if(currentStep==0){
|
||||||
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
||||||
@ -335,10 +287,58 @@ public:
|
|||||||
}else{
|
}else{
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
}
|
}
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
|
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
|
currentStep++;
|
||||||
|
c=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
//Lower Shooter{{{
|
||||||
|
if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
|
||||||
|
if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
|
||||||
|
shootRobot(-0.3f);
|
||||||
|
}else{
|
||||||
|
shootRobot(0.0f);
|
||||||
|
}
|
||||||
|
if(c==(3.0f)*200){
|
||||||
|
shootRobot(0.0f);
|
||||||
|
currentStep++;
|
||||||
|
c=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
break;
|
||||||
|
//}}}
|
||||||
|
//Autonomous1{{{
|
||||||
|
case 1:
|
||||||
|
//Drive{{{
|
||||||
|
if(currentStep==0){
|
||||||
|
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
||||||
|
driveRobot(-1.0f,correction);
|
||||||
|
}else{
|
||||||
|
driveRobot(0.0f,0.0f);
|
||||||
|
}
|
||||||
|
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
|
||||||
|
driveRobot(0.0f,0.0f);
|
||||||
|
currentStep++;
|
||||||
|
c=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//}}}
|
||||||
|
//Shoot{{{
|
||||||
|
if(currentStep==1&&c>SmartDashboard::GetNumber("First 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){
|
||||||
|
shootRobot(0.0f);
|
||||||
|
setMotorValue(6,1,0);
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
}
|
}
|
||||||
@ -381,29 +381,29 @@ public:
|
|||||||
}else{
|
}else{
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
}
|
}
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
|
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
break;
|
break;
|
||||||
//}}}
|
//}}}
|
||||||
//Autonomous2{{{
|
//Autonomous2{{{
|
||||||
case 2:
|
case 2:
|
||||||
//Drive{{{
|
//Drive{{{
|
||||||
if(currentStep==0&&c>SmartDashboard::GetNumber("Initial Drive Delay")*200){
|
if(currentStep==0&&c>SmartDashboard::GetNumber("Initial Drive Delay")*200){
|
||||||
setMotorValue(6, 1, SmartDashboard::GetNumber("Collector Speed"));
|
setMotorValue(6,1,SmartDashboard::GetNumber("Collector Speed"));
|
||||||
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=SmartDashboard::GetNumber("Auto Distance")){
|
||||||
driveRobot(-1*(SmartDashboard::GetNumber("AutoSpeed")),correction);
|
driveRobot(-1*(SmartDashboard::GetNumber("AutoSpeed")),correction);
|
||||||
}else{
|
}else{
|
||||||
driveRobot(0.0f,0.0f);
|
driveRobot(0.0f,0.0f);
|
||||||
}
|
}
|
||||||
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
|
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
driveRobot(0.0f,0.0f);
|
driveRobot(0.0f,0.0f);
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
@ -412,7 +412,7 @@ public:
|
|||||||
//}}}
|
//}}}
|
||||||
//Release Ball{{{
|
//Release Ball{{{
|
||||||
if(currentStep==1){
|
if(currentStep==1){
|
||||||
setMotorValue(6, 1, 102);
|
setMotorValue(6,1,102);
|
||||||
if(c==50){
|
if(c==50){
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
@ -428,7 +428,7 @@ public:
|
|||||||
}
|
}
|
||||||
if(c==SmartDashboard::GetNumber("First Shot Stop")*200){
|
if(c==SmartDashboard::GetNumber("First Shot Stop")*200){
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
}
|
}
|
||||||
@ -450,9 +450,9 @@ public:
|
|||||||
//}}}
|
//}}}
|
||||||
//Collect Ball{{{
|
//Collect Ball{{{
|
||||||
if(currentStep==4&&c>SmartDashboard::GetNumber("Second Shot Start")){
|
if(currentStep==4&&c>SmartDashboard::GetNumber("Second Shot Start")){
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
if(c==SmartDashboard::GetNumber("Second Shot Stop")*200){
|
if(c==SmartDashboard::GetNumber("Second Shot Stop")*200){
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
}
|
}
|
||||||
@ -465,9 +465,9 @@ public:
|
|||||||
}else{
|
}else{
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
}
|
}
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
|
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
currentStep++;
|
currentStep++;
|
||||||
c=0;
|
c=0;
|
||||||
@ -477,47 +477,47 @@ public:
|
|||||||
break;
|
break;
|
||||||
//}}}
|
//}}}
|
||||||
}
|
}
|
||||||
SmartDashboard::PutNumber("Autonomous step", currentStep);
|
SmartDashboard::PutNumber("Autonomous step",currentStep);
|
||||||
updateDashboard();
|
updateDashboard();
|
||||||
//Compressor{{{
|
//Compressor{{{
|
||||||
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
|
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1){
|
||||||
compressor.Stop();
|
compressor.Stop();
|
||||||
compressing = false;
|
compressing=false;
|
||||||
}
|
}
|
||||||
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
|
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0){
|
||||||
compressor.Start();
|
compressor.Start();
|
||||||
compressing = true;
|
compressing=true;
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
i++;
|
i++;
|
||||||
c++;
|
c++;
|
||||||
Wait(0.005f);
|
Wait(0.005f);
|
||||||
}
|
}
|
||||||
compressing = false;
|
compressing=false;
|
||||||
compressor.Stop();
|
compressor.Stop();
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//Teleop{{{
|
//Teleop{{{
|
||||||
void OperatorControl() {
|
void OperatorControl(){
|
||||||
//Initializations{{{
|
//Initializations{{{
|
||||||
myRobot.SetSafetyEnabled(false);
|
myRobot.SetSafetyEnabled(false);
|
||||||
int i = 0;
|
int i=0;
|
||||||
collectorSole1.Set(true);
|
collectorSole1.Set(true);
|
||||||
collectorSole2.Set(false);
|
collectorSole2.Set(false);
|
||||||
compressing = false;
|
compressing=false;
|
||||||
SmartDashboard::PutBoolean("CollectorState",false);
|
SmartDashboard::PutBoolean("CollectorState",false);
|
||||||
//}}}
|
//}}}
|
||||||
while(IsEnabled() && IsOperatorControl()) {
|
while(IsEnabled() && IsOperatorControl()){
|
||||||
//Joystick{{{
|
//Joystick{{{
|
||||||
//Throttle values{{{
|
//Throttle values{{{
|
||||||
if(Lstick.GetRawButton(9)==1){
|
if(Lstick.GetRawButton(9)==1){
|
||||||
throttle = (-Lstick.GetRawAxis(4)+1)/2;
|
throttle=(-Lstick.GetRawAxis(4)+1)/2;
|
||||||
}else if(Lstick.GetRawButton(10)){
|
}else if(Lstick.GetRawButton(10)){
|
||||||
throttle = SmartDashboard::GetNumber("ShooterButtonPower10");
|
throttle=SmartDashboard::GetNumber("ShooterButtonPower10");
|
||||||
}else if(Lstick.GetRawButton(7)){
|
}else if(Lstick.GetRawButton(7)){
|
||||||
throttle = SmartDashboard::GetNumber("ShooterButtonPower7");
|
throttle=SmartDashboard::GetNumber("ShooterButtonPower7");
|
||||||
}else if(Lstick.GetRawButton(8)){
|
}else if(Lstick.GetRawButton(8)){
|
||||||
throttle = SmartDashboard::GetNumber("ShooterButtonPower8");
|
throttle=SmartDashboard::GetNumber("ShooterButtonPower8");
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//Shooter upLimit{{{
|
//Shooter upLimit{{{
|
||||||
@ -534,60 +534,60 @@ public:
|
|||||||
upLimit=130.0f;
|
upLimit=130.0f;
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
if(Lstick.GetRawButton(1)==1) {
|
if(Lstick.GetRawButton(1)==1){
|
||||||
//Shoot{{{
|
//Shoot{{{
|
||||||
shooting = true;
|
shooting=true;
|
||||||
shootRobot(throttle);
|
shootRobot(throttle);
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
if(collectorExtended == false) {
|
if(collectorExtended == false){
|
||||||
shooting = false;
|
shooting=false;
|
||||||
}
|
}
|
||||||
if(collectorExtended == true&&(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage()))) {
|
if(collectorExtended == true&&(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage()))){
|
||||||
shooting = true;
|
shooting=true;
|
||||||
shootRobot(throttle);
|
shootRobot(throttle);
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
}else{
|
}else{
|
||||||
shooting = false;
|
shooting=false;
|
||||||
shootRobot(0.0f);
|
shootRobot(0.0f);
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
} else if(Lstick.GetRawButton(2)==1) {
|
}else if(Lstick.GetRawButton(2)==1){
|
||||||
//Lower Shooter{{{
|
//Lower Shooter{{{
|
||||||
shooting = false;
|
shooting=false;
|
||||||
shootRobot(-0.1f);
|
shootRobot(-0.1f);
|
||||||
if(collectorExtended == true) {
|
if(collectorExtended == true){
|
||||||
shootRobot(-0.1f);
|
shootRobot(-0.1f);
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
} else {
|
}else{
|
||||||
//Stop Shooting{{{
|
//Stop Shooting{{{
|
||||||
shooting = false;
|
shooting=false;
|
||||||
shootRobot(0);
|
shootRobot(0);
|
||||||
//}}}
|
//}}}
|
||||||
}
|
}
|
||||||
if(Rstick.GetRawButton(9)==1) {
|
if(Rstick.GetRawButton(9)==1){
|
||||||
//Extend Collector{{{
|
//Extend Collector{{{
|
||||||
SmartDashboard::PutBoolean("CollectorState",true);
|
SmartDashboard::PutBoolean("CollectorState",true);
|
||||||
collectorExtended = true;
|
collectorExtended=true;
|
||||||
collectorSole1.Set(false);
|
collectorSole1.Set(false);
|
||||||
collectorSole2.Set(true);
|
collectorSole2.Set(true);
|
||||||
//}}}
|
//}}}
|
||||||
} else if(Rstick.GetRawButton(10)==1) {
|
}else if(Rstick.GetRawButton(10)==1){
|
||||||
//Retract Collector{{{
|
//Retract Collector{{{
|
||||||
SmartDashboard::PutBoolean("CollectorState",false);
|
SmartDashboard::PutBoolean("CollectorState",false);
|
||||||
collectorExtended = false;
|
collectorExtended=false;
|
||||||
collectorSole1.Set(true);
|
collectorSole1.Set(true);
|
||||||
collectorSole2.Set(false);
|
collectorSole2.Set(false);
|
||||||
//}}}
|
//}}}
|
||||||
}
|
}
|
||||||
//Collector Motor{{{
|
//Collector Motor{{{
|
||||||
if(Lstick.GetRawButton(11)==1) {
|
if(Lstick.GetRawButton(11)==1){
|
||||||
setMotorValue(6, 1, 1);
|
setMotorValue(6,1,1);
|
||||||
} else if(Lstick.GetRawButton(12)==1) {
|
}else if(Lstick.GetRawButton(12)==1){
|
||||||
setMotorValue(6, 1, 255);
|
setMotorValue(6,1,255);
|
||||||
} else if(!shooting) {
|
}else if(!shooting){
|
||||||
setMotorValue(6, 1, 0);
|
setMotorValue(6,1,0);
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//}}}
|
//}}}
|
||||||
@ -599,22 +599,22 @@ public:
|
|||||||
driveRobot(0.0f,0.0f);
|
driveRobot(0.0f,0.0f);
|
||||||
}
|
}
|
||||||
}else{
|
}else{
|
||||||
if(SmartDashboard::GetBoolean("Daniel Mode")) {
|
if(SmartDashboard::GetBoolean("Daniel Mode")){
|
||||||
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
||||||
} else {
|
}else{
|
||||||
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//Compressor{{{
|
//Compressor{{{
|
||||||
if(SmartDashboard::GetBoolean("Compressor Enabled")){
|
if(SmartDashboard::GetBoolean("Compressor Enabled")){
|
||||||
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
|
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1){
|
||||||
compressor.Stop();
|
compressor.Stop();
|
||||||
compressing = false;
|
compressing=false;
|
||||||
}
|
}
|
||||||
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
|
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0){
|
||||||
compressor.Start();
|
compressor.Start();
|
||||||
compressing = true;
|
compressing=true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
@ -622,12 +622,12 @@ public:
|
|||||||
i++;
|
i++;
|
||||||
Wait(0.005f);
|
Wait(0.005f);
|
||||||
}
|
}
|
||||||
compressing = false;
|
compressing=false;
|
||||||
compressor.Stop();
|
compressor.Stop();
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
//Test{{{
|
//Test{{{
|
||||||
void Test() {
|
void Test(){
|
||||||
}
|
}
|
||||||
//}}}
|
//}}}
|
||||||
};
|
};
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user