team2059/zaphod
2
0
Fork 0
mirror of https://github.com/team2059/Zaphod synced 2025-01-07 22:14:14 -05:00
Code Issues Releases Wiki Activity

Fixed up code after car voyage... Preparing to add ultrasonic code

Browse Source
This commit is contained in:
Austen Adler 2014-02-27 04:02:49 +00:00
parent 938a8b74f8
commit 0b57a289ce
1 changed files with 125 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

318
MyRobot.cpp
View File

@ -1,4 +1,3 @@
//TODO this
//Sonar code //Sonar code
#include "WPILib.h" #include "WPILib.h"
#include "SmartDashboard/SmartDashboard.h" #include "SmartDashboard/SmartDashboard.h"
@ -27,8 +26,12 @@ class RobotDemo : public SimpleRobot
//Ultrasonic //Ultrasonic
AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight; AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
DigitalOutput BallLeft, BallRight, WallLeft, WallRight; DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
//DigitalInput ballLimit;
public: public:
RobotDemo(): RobotDemo():
//Limit Switches
//TODO
//ballLimit(),
//Joysticks //Joysticks
Rstick(1), Rstick(1),
Lstick(2), Lstick(2),
@ -44,8 +47,8 @@ public:
BallRight(2,4), BallRight(2,4),
WallRight(2,5), WallRight(2,5),
//Compressor //Compressor
//compressor(2,3,1,1), compressor(2, 5, 1, 1),
compressor(2, 5, 1, 1), //Solenoids //Solenoids
collectorSole1(1), collectorSole1(1),
collectorSole2(2), collectorSole2(2),
//Limit switches //Limit switches
@ -105,7 +108,7 @@ public:
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::PutBoolean("Use Ultrasonic",false); SmartDashboard::PutBoolean("Use Ultrasonic",false);
SmartDashboard::PutBoolean("Reverse Robot",false); SmartDashboard::PutBoolean("Daniel Mode",false);
} }
void updateDashboard() { void updateDashboard() {
SmartDashboard::PutNumber("Throttle", throttle); SmartDashboard::PutNumber("Throttle", throttle);
@ -118,10 +121,10 @@ public:
DownSpeed = SmartDashboard::GetNumber("DownSpeed"); DownSpeed = SmartDashboard::GetNumber("DownSpeed");
downLimit = SmartDashboard::GetNumber("downLimit"); downLimit = SmartDashboard::GetNumber("downLimit");
upLimit = SmartDashboard::GetNumber("upLimit"); upLimit = SmartDashboard::GetNumber("upLimit");
if (downLimit < 35) { if(downLimit < 35) {
downLimit = 35; downLimit = 35;
} }
if (upLimit > 167) { if(upLimit > 167) {
upLimit = 167; upLimit = 167;
} }
} }
@ -131,20 +134,20 @@ public:
//In this case its checking that we are no more than 15 degrees off //In this case its checking that we are no more than 15 degrees off
//The override is in place in case an ultrasonic becomes damaged and we are unable to validate the distance through software //The override is in place in case an ultrasonic becomes damaged and we are unable to validate the distance through software
float averageAtan = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f); float averageAtan = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f);
if(override==true){ if(override==true) {
printf("%f\n",power); printf("%f\n",power);
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));
}else if(averageAtan<=30.0f&&(voltToDistance(WallSonicLeft.GetAverageVoltage(),true))<=100&&(voltToDistance(WallSonicRight.GetAverageVoltage(),true))<=100){ } else if(averageAtan<=30.0f&&(voltToDistance(WallSonicLeft.GetAverageVoltage(),true))<=100&&(voltToDistance(WallSonicRight.GetAverageVoltage(),true))<=100) {
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));
} }
if(averageAtan>=16.0f&&(voltToDistance(WallSonicLeft.GetAverageVoltage(),true))<=100&&(voltToDistance(WallSonicRight.GetAverageVoltage(),true))){ if(averageAtan>=16.0f&&(voltToDistance(WallSonicLeft.GetAverageVoltage(),true))<=100&&(voltToDistance(WallSonicRight.GetAverageVoltage(),true))) {
//printf/smartdashboard: warning //printf/smartdashboard: warning
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));
@ -152,15 +155,15 @@ public:
} }
//Allow shooting regardless of the angle or distance //Allow shooting regardless of the angle or distance
} }
void logMsg(std::string message, int level){ void logMsg(std::string message, int level) {
if((int)SmartDashboard::GetNumber("Log Level") % level == 0){ if((int)SmartDashboard::GetNumber("Log Level") % level == 0) {
printf((message+"\n").c_str()); printf((message+"\n").c_str());
} }
} }
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 leftPower = ((y+x)/2+1)*127+1;
@ -176,7 +179,7 @@ public:
setMotorValue(2, 2, rightPower); setMotorValue(2, 2, rightPower);
setMotorValue(3, 2, rightPower); setMotorValue(3, 2, rightPower);
} }
template<typename numbertype> string toString(numbertype a){ template<typename numbertype> string toString(numbertype a) {
//TODO //TODO
stringstream ss; stringstream ss;
ss<<a; ss<<a;
@ -184,9 +187,9 @@ public:
return s; return s;
} }
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;
} }
} }
@ -202,66 +205,66 @@ public:
return input * 127.0f + 128; return input * 127.0f + 128;
} }
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) {
switch (motor) { switch(motor) {
//Drive motors //Drive motors
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) {
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:
Servo1.SetAngle(value); Servo1.SetAngle(value);
break; break;
case 8: case 8:
Servo2.SetAngle(value); Servo2.SetAngle(value);
break; break;
case 9: case 9:
break; break;
case 10: case 10:
break; break;
} }
} }
} }
@ -269,8 +272,6 @@ public:
float yoyo = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f); float yoyo = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f);
SmartDashboard::PutNumber("Distance", yoyo); SmartDashboard::PutNumber("Distance", yoyo);
} }
// float getSonar(int sonar){
// switch(sonar){
void Test() { void Test() {
} }
void Autonomous() { void Autonomous() {
@ -298,52 +299,57 @@ public:
BallLeft.Set(0); BallLeft.Set(0);
WallRight.Set(1); WallRight.Set(1);
BallRight.Set(0); BallRight.Set(0);
while(IsEnabled()&&IsAutonomous()){ while(IsEnabled()&&IsAutonomous()) {
if(SmartDashboard::GetBoolean("Use Ultrasonic")){ if(SmartDashboard::GetBoolean("Use Ultrasonic")) {
if(cur<sampleCount) { if(cur<sampleCount) {
WallLeft.Set(1); WallLeft.Set(1);
WallRight.Set(0); WallRight.Set(0);
avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true); avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true);
}else if(cur==sampleCount){ } else if(cur==sampleCount) {
avgLeft/=12; avgLeft/=12;
}else if(cur<sampleCount*4&&cur>3*sampleCount){ } else if(cur<sampleCount*4&&cur>3*sampleCount) {
WallLeft.Set(0); WallLeft.Set(0);
WallRight.Set(1); WallRight.Set(1);
avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true); avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true);
}else if(cur==sampleCount*4){ } else if(cur==sampleCount*4) {
avgRight/=12; avgRight/=12;
thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f); thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f);
}else if(cur==sampleCount*6){ } else if(cur==sampleCount*6) {
cur=0; cur=0;
} }
if(i>164){ if(i>164) {
SmartDashboard::PutNumber("Tanval", thisIsATest); SmartDashboard::PutNumber("Tanval", thisIsATest);
} }
}else{ } else {
if (i<=initalDriveTime*200) { if(i<=initalDriveTime*200) {
setMotorValue(6, 1, 1); driveRobot(1.0f,0.0f);
} else if (i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) { //setMotorValue(6, 1, 1);
} else if(i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) {
driveRobot(0, 0); driveRobot(0, 0);
shootRobot(1, true); shootRobot(1, true);
setMotorValue(6, 1, 1); setMotorValue(6, 1, 1);
} else if (i>1500&&i<1700) { }
shootRobot(.1,true); if(i>480&&i<430+initalDriveTime){
//driveRobot(-1,0); driveRobot(-1,0);
} else { //TODO
/* } else if(i>430+initalDriveTime/*&&ballLimit.Get()==1*/) {
driveRobot(0, 0); driveRobot(-0.6f,0.0f);
shootRobot(0, true); setMotorValue(6, 1, 1);
setMotorValue(6, 1, 0); //TODO
*/ } else if(/*ballLimit.Get()==1&&*/i>430+initalDriveTime) {
driveRobot(1.0f,0.0f);
}
if(i>480&&i<480){
shootRobot(0.25f,true);
} }
} }
updateDashboard(); updateDashboard();
if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) { if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop(); compressor.Stop();
compressing = false; compressing = false;
logMsg("Stopping the compressor",2); logMsg("Stopping the compressor",2);
} }
if (i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) { if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
compressor.Start(); compressor.Start();
compressing = true; compressing = true;
logMsg("Starting the compressor",2); logMsg("Starting the compressor",2);
@ -354,80 +360,6 @@ public:
} }
compressing = false; compressing = false;
compressor.Stop(); compressor.Stop();
// while (IsEnabled() && IsAutonomous()) {
// //Collect left average values from cur values 0 to 12
// if(cur<12){
// WallLeft.Set(1);
// BallLeft.Set(1);
// WallRight.Set(0);
// BallRight.Set(0);
// avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true);
// //After 12 cur iterations, find the average (data was recorded 12 times, divide by 12)
// }else if(cur==12){
// avgLeft/=12;
// //Collect right average values from cur values 38 to 50
// }else if(cur<100&&cur>88){
// WallLeft.Set(0);
// BallLeft.Set(0);
// WallRight.Set(1);
// BallRight.Set(1);
// avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true);
// //After 50 cur iterations, find the average (data was recorded 12 times, divide by 12)
// }else if(cur==100){
// cur=0;
// avgRight/=12;
// thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f);
// SmartDashboard::PutNumber("Tanval", atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f));
// }
// //Calculate the inital distance and average it averageAmount times.
// if(i<averageAmount){
// avgDist+=(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)+voltToDistance(WallSonicRight.GetAverageVoltage(),true)/2);
// }else{
// avgDist/=averageAmount;
// }
// //Calculate the average distance from the wall
// curDist=thisIsATest;
// if(i%100==0){
// printf("Difference: %f\n",avgDist-curDist);
// }
// if (i>=5&&avgDist-curDist<=36.0f) {
// float xPower, yPower;
// xPower=1;
// yPower=(avgDist-curDist)/36.0f;
// if(yPower>1){
// yPower=1;
// }
// driveRobot(yPower, xPower);
// setMotorValue(6, 1, 1);
// } else if (i>1400&&i<1600&&125>=potToDegrees(armPot.GetAverageVoltage())) {
// driveRobot(0, 0);
// shootRobot(1, false);
// setMotorValue(6, 1, 1);
// } else if (i>1500&&i<1700) {
// shootRobot(.1,false);
// //driveRobot(-1,0);
// } else {
// /*
// driveRobot(0, 0);
// shootRobot(0, true);
// setMotorValue(6, 1, 0);
// */
// }
// updateDashboard();
// if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
// compressor.Stop();
// compressing = false;
// logMsg("Stopping the compressor",2);
// }
// if (i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
// compressor.Start();
// compressing = true;
// logMsg("Starting the compressor",2);
// }
// Wait(0.005f);
// i++;
// cur++;
//}
} }
void OperatorControl() { void OperatorControl() {
myRobot.SetSafetyEnabled(false); myRobot.SetSafetyEnabled(false);
@ -438,8 +370,8 @@ public:
collectorSole2.Set(false); collectorSole2.Set(false);
compressing = false; compressing = false;
logMsg("Starting Teleop",1); logMsg("Starting Teleop",1);
while (IsEnabled() && IsOperatorControl()) { while(IsEnabled() && IsOperatorControl()) {
if(cur==50){ if(cur==50) {
cur=0; cur=0;
WallLeft.Set(swap?1:0); WallLeft.Set(swap?1:0);
BallRight.Set(swap?1:0); BallRight.Set(swap?1:0);
@ -448,31 +380,31 @@ public:
swap=!swap; swap=!swap;
} }
throttle = (-Lstick.GetRawAxis(4)+1)/2; throttle = (-Lstick.GetRawAxis(4)+1)/2;
if(SmartDashboard::GetBoolean("Reverse Robot")){ 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());
} }
//Log things //Log things
if (i % 200 == 0) { if(i % 200 == 0) {
//logMsg(toString(compressor.GetPressureSwitchValue()),2); //logMsg(toString(compressor.GetPressureSwitchValue()),2);
//logMsg("armPot value: "+toString(armPot.GetAverageVoltage(),11)); //logMsg("armPot value: "+toString(armPot.GetAverageVoltage(),11));
//logMsg("Converted armPot value: "+toString(armPot.GetAverageVoltage(),11)); //logMsg("Converted armPot value: "+toString(armPot.GetAverageVoltage(),11));
} }
if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) { if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop(); compressor.Stop();
compressing = false; compressing = false;
logMsg("Stopping the compressor",2); logMsg("Stopping the compressor",2);
} }
if (i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) { if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
compressor.Start(); compressor.Start();
compressing = true; compressing = true;
logMsg("Starting the compressor... again",2); logMsg("Starting the compressor... again",2);
} }
updateDashboard(); updateDashboard();
if (Lstick.GetRawButton(1)==1&&(upLimit>=potToDegrees(armPot.GetAverageVoltage()))) { if(Lstick.GetRawButton(1)==1&&(upLimit>=potToDegrees(armPot.GetAverageVoltage()))) {
//Move arm motors based on throttle //Move arm motors based on throttle
if (collectorExtended == false) { if(collectorExtended == false) {
shooting = false; shooting = false;
logMsg("Collector is NOT extended, not going to fire",17); logMsg("Collector is NOT extended, not going to fire",17);
} }
@ -488,34 +420,35 @@ public:
logMsg("Stopping shooter motor",13); logMsg("Stopping shooter motor",13);
logMsg("Stopping collector motor",17); logMsg("Stopping collector motor",17);
shootRobot(0, true); shootRobot(0, true);
} else if (Lstick.GetRawButton(2)==1) { } else if(Lstick.GetRawButton(2)==1) {
//Reverse the arm motors //Reverse the arm motors
shooting = false; shooting = false;
if (collectorExtended == false) { if(collectorExtended == false) {
logMsg("Collector is not extended, not going to fire",17); logMsg("Collector is not extended, not going to fire",17);
} }
if (collectorExtended == true) { if(collectorExtended == true) {
shootRobot(-DownSpeed, false); shootRobot(-DownSpeed, false);
logMsg("Collector is extended, going to fire",17); logMsg("Collector is extended, going to fire",17);
} } else { }
} else {
shooting = false; shooting = false;
//Stop all motors //Stop all motors
shootRobot(0, true); shootRobot(0, true);
} }
if (Rstick.GetRawButton(9)==1) { if(Rstick.GetRawButton(9)==1) {
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) {
collectorExtended = false; collectorExtended = false;
collectorSole1.Set(true); collectorSole1.Set(true);
collectorSole2.Set(false); collectorSole2.Set(false);
} }
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);
} }
cur++; cur++;
@ -525,4 +458,3 @@ public:
} }
}; };
START_ROBOT_CLASS(RobotDemo); START_ROBOT_CLASS(RobotDemo);