mirror of
https://github.com/team2059/Zaphod
synced 2025-01-07 22:14:14 -05:00
Fixed up code after car voyage... Preparing to add ultrasonic code
This commit is contained in:
parent
938a8b74f8
commit
0b57a289ce
318
MyRobot.cpp
318
MyRobot.cpp
@ -1,4 +1,3 @@
|
||||
//TODO this
|
||||
//Sonar code
|
||||
#include "WPILib.h"
|
||||
#include "SmartDashboard/SmartDashboard.h"
|
||||
@ -27,8 +26,12 @@ class RobotDemo : public SimpleRobot
|
||||
//Ultrasonic
|
||||
AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
|
||||
DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
|
||||
//DigitalInput ballLimit;
|
||||
public:
|
||||
RobotDemo():
|
||||
//Limit Switches
|
||||
//TODO
|
||||
//ballLimit(),
|
||||
//Joysticks
|
||||
Rstick(1),
|
||||
Lstick(2),
|
||||
@ -44,8 +47,8 @@ public:
|
||||
BallRight(2,4),
|
||||
WallRight(2,5),
|
||||
//Compressor
|
||||
//compressor(2,3,1,1),
|
||||
compressor(2, 5, 1, 1), //Solenoids
|
||||
compressor(2, 5, 1, 1),
|
||||
//Solenoids
|
||||
collectorSole1(1),
|
||||
collectorSole2(2),
|
||||
//Limit switches
|
||||
@ -105,7 +108,7 @@ public:
|
||||
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
|
||||
SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
|
||||
SmartDashboard::PutBoolean("Use Ultrasonic",false);
|
||||
SmartDashboard::PutBoolean("Reverse Robot",false);
|
||||
SmartDashboard::PutBoolean("Daniel Mode",false);
|
||||
}
|
||||
void updateDashboard() {
|
||||
SmartDashboard::PutNumber("Throttle", throttle);
|
||||
@ -118,10 +121,10 @@ public:
|
||||
DownSpeed = SmartDashboard::GetNumber("DownSpeed");
|
||||
downLimit = SmartDashboard::GetNumber("downLimit");
|
||||
upLimit = SmartDashboard::GetNumber("upLimit");
|
||||
if (downLimit < 35) {
|
||||
if(downLimit < 35) {
|
||||
downLimit = 35;
|
||||
}
|
||||
if (upLimit > 167) {
|
||||
if(upLimit > 167) {
|
||||
upLimit = 167;
|
||||
}
|
||||
}
|
||||
@ -131,20 +134,20 @@ public:
|
||||
//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
|
||||
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);
|
||||
setMotorValue(4, 1, cvt(power));
|
||||
setMotorValue(5, 1, cvt(power));
|
||||
setMotorValue(4, 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(5, 1, cvt(power));
|
||||
setMotorValue(4, 2, cvt(-power));
|
||||
setMotorValue(5, 2, cvt(-power));
|
||||
}
|
||||
if(averageAtan>=16.0f&&(voltToDistance(WallSonicLeft.GetAverageVoltage(),true))<=100&&(voltToDistance(WallSonicRight.GetAverageVoltage(),true))){
|
||||
//printf/smartdashboard: warning
|
||||
if(averageAtan>=16.0f&&(voltToDistance(WallSonicLeft.GetAverageVoltage(),true))<=100&&(voltToDistance(WallSonicRight.GetAverageVoltage(),true))) {
|
||||
//printf/smartdashboard: warning
|
||||
setMotorValue(4, 1, cvt(power));
|
||||
setMotorValue(5, 1, cvt(power));
|
||||
setMotorValue(4, 2, cvt(-power));
|
||||
@ -152,15 +155,15 @@ public:
|
||||
}
|
||||
//Allow shooting regardless of the angle or distance
|
||||
}
|
||||
void logMsg(std::string message, int level){
|
||||
if((int)SmartDashboard::GetNumber("Log Level") % level == 0){
|
||||
void logMsg(std::string message, int level) {
|
||||
if((int)SmartDashboard::GetNumber("Log Level") % level == 0) {
|
||||
printf((message+"\n").c_str());
|
||||
}
|
||||
}
|
||||
void driveRobot(float x, float y) {
|
||||
if(y>1.0f){
|
||||
if(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;
|
||||
}
|
||||
int leftPower = ((y+x)/2+1)*127+1;
|
||||
@ -176,7 +179,7 @@ public:
|
||||
setMotorValue(2, 2, rightPower);
|
||||
setMotorValue(3, 2, rightPower);
|
||||
}
|
||||
template<typename numbertype> string toString(numbertype a){
|
||||
template<typename numbertype> string toString(numbertype a) {
|
||||
//TODO
|
||||
stringstream ss;
|
||||
ss<<a;
|
||||
@ -184,9 +187,9 @@ public:
|
||||
return s;
|
||||
}
|
||||
float voltToDistance(float a,bool wall=false) {
|
||||
if(wall){
|
||||
if(wall) {
|
||||
return (a / 0.00488f) / 2.54f;
|
||||
}else{
|
||||
} else {
|
||||
return (a / 0.000976562f) / 25.4f;
|
||||
}
|
||||
}
|
||||
@ -202,66 +205,66 @@ public:
|
||||
return input * 127.0f + 128;
|
||||
}
|
||||
void setMotorValue(int motor, int subwayStation = 1, int value = 127) {
|
||||
if (subwayStation == 1) {
|
||||
switch (motor) {
|
||||
//Drive motors
|
||||
case 1:
|
||||
Left1.SetRaw(value);
|
||||
break;
|
||||
case 2:
|
||||
Left2.SetRaw(value);
|
||||
break;
|
||||
case 3:
|
||||
Left3.SetRaw(value);
|
||||
break;
|
||||
case 4:
|
||||
LeftArmMotor1.SetRaw(value);
|
||||
break;
|
||||
case 5:
|
||||
LeftArmMotor2.SetRaw(value);
|
||||
break;
|
||||
case 6:
|
||||
CollectorMotor1.SetRaw(value);
|
||||
break;
|
||||
case 7:
|
||||
break;
|
||||
case 8:
|
||||
break;
|
||||
case 9:
|
||||
break;
|
||||
case 10:
|
||||
break;
|
||||
if(subwayStation == 1) {
|
||||
switch(motor) {
|
||||
//Drive motors
|
||||
case 1:
|
||||
Left1.SetRaw(value);
|
||||
break;
|
||||
case 2:
|
||||
Left2.SetRaw(value);
|
||||
break;
|
||||
case 3:
|
||||
Left3.SetRaw(value);
|
||||
break;
|
||||
case 4:
|
||||
LeftArmMotor1.SetRaw(value);
|
||||
break;
|
||||
case 5:
|
||||
LeftArmMotor2.SetRaw(value);
|
||||
break;
|
||||
case 6:
|
||||
CollectorMotor1.SetRaw(value);
|
||||
break;
|
||||
case 7:
|
||||
break;
|
||||
case 8:
|
||||
break;
|
||||
case 9:
|
||||
break;
|
||||
case 10:
|
||||
break;
|
||||
}
|
||||
} else if (subwayStation == 2) {
|
||||
switch (motor) {
|
||||
//Shooter motors
|
||||
case 1:
|
||||
Right1.SetRaw(value);
|
||||
break;
|
||||
case 2:
|
||||
Right2.SetRaw(value);
|
||||
break;
|
||||
case 3:
|
||||
Right3.SetRaw(value);
|
||||
break;
|
||||
case 4:
|
||||
RightArmMotor1.SetRaw(value);
|
||||
break;
|
||||
case 5:
|
||||
RightArmMotor2.SetRaw(value);
|
||||
break;
|
||||
case 6:
|
||||
break;
|
||||
case 7:
|
||||
Servo1.SetAngle(value);
|
||||
break;
|
||||
case 8:
|
||||
Servo2.SetAngle(value);
|
||||
break;
|
||||
case 9:
|
||||
break;
|
||||
case 10:
|
||||
break;
|
||||
} else if(subwayStation == 2) {
|
||||
switch(motor) {
|
||||
//Shooter motors
|
||||
case 1:
|
||||
Right1.SetRaw(value);
|
||||
break;
|
||||
case 2:
|
||||
Right2.SetRaw(value);
|
||||
break;
|
||||
case 3:
|
||||
Right3.SetRaw(value);
|
||||
break;
|
||||
case 4:
|
||||
RightArmMotor1.SetRaw(value);
|
||||
break;
|
||||
case 5:
|
||||
RightArmMotor2.SetRaw(value);
|
||||
break;
|
||||
case 6:
|
||||
break;
|
||||
case 7:
|
||||
Servo1.SetAngle(value);
|
||||
break;
|
||||
case 8:
|
||||
Servo2.SetAngle(value);
|
||||
break;
|
||||
case 9:
|
||||
break;
|
||||
case 10:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -269,8 +272,6 @@ public:
|
||||
float yoyo = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f);
|
||||
SmartDashboard::PutNumber("Distance", yoyo);
|
||||
}
|
||||
// float getSonar(int sonar){
|
||||
// switch(sonar){
|
||||
void Test() {
|
||||
}
|
||||
void Autonomous() {
|
||||
@ -298,52 +299,57 @@ public:
|
||||
BallLeft.Set(0);
|
||||
WallRight.Set(1);
|
||||
BallRight.Set(0);
|
||||
while(IsEnabled()&&IsAutonomous()){
|
||||
if(SmartDashboard::GetBoolean("Use Ultrasonic")){
|
||||
while(IsEnabled()&&IsAutonomous()) {
|
||||
if(SmartDashboard::GetBoolean("Use Ultrasonic")) {
|
||||
if(cur<sampleCount) {
|
||||
WallLeft.Set(1);
|
||||
WallRight.Set(0);
|
||||
avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true);
|
||||
}else if(cur==sampleCount){
|
||||
} else if(cur==sampleCount) {
|
||||
avgLeft/=12;
|
||||
}else if(cur<sampleCount*4&&cur>3*sampleCount){
|
||||
} else if(cur<sampleCount*4&&cur>3*sampleCount) {
|
||||
WallLeft.Set(0);
|
||||
WallRight.Set(1);
|
||||
avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true);
|
||||
}else if(cur==sampleCount*4){
|
||||
} else if(cur==sampleCount*4) {
|
||||
avgRight/=12;
|
||||
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;
|
||||
}
|
||||
if(i>164){
|
||||
if(i>164) {
|
||||
SmartDashboard::PutNumber("Tanval", thisIsATest);
|
||||
}
|
||||
}else{
|
||||
if (i<=initalDriveTime*200) {
|
||||
setMotorValue(6, 1, 1);
|
||||
} else if (i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) {
|
||||
} else {
|
||||
if(i<=initalDriveTime*200) {
|
||||
driveRobot(1.0f,0.0f);
|
||||
//setMotorValue(6, 1, 1);
|
||||
} else if(i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) {
|
||||
driveRobot(0, 0);
|
||||
shootRobot(1, true);
|
||||
setMotorValue(6, 1, 1);
|
||||
} else if (i>1500&&i<1700) {
|
||||
shootRobot(.1,true);
|
||||
//driveRobot(-1,0);
|
||||
} else {
|
||||
/*
|
||||
driveRobot(0, 0);
|
||||
shootRobot(0, true);
|
||||
setMotorValue(6, 1, 0);
|
||||
*/
|
||||
}
|
||||
if(i>480&&i<430+initalDriveTime){
|
||||
driveRobot(-1,0);
|
||||
//TODO
|
||||
} else if(i>430+initalDriveTime/*&&ballLimit.Get()==1*/) {
|
||||
driveRobot(-0.6f,0.0f);
|
||||
setMotorValue(6, 1, 1);
|
||||
//TODO
|
||||
} else if(/*ballLimit.Get()==1&&*/i>430+initalDriveTime) {
|
||||
driveRobot(1.0f,0.0f);
|
||||
}
|
||||
if(i>480&&i<480){
|
||||
shootRobot(0.25f,true);
|
||||
}
|
||||
}
|
||||
updateDashboard();
|
||||
if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
|
||||
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) {
|
||||
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
|
||||
compressor.Start();
|
||||
compressing = true;
|
||||
logMsg("Starting the compressor",2);
|
||||
@ -354,80 +360,6 @@ public:
|
||||
}
|
||||
compressing = false;
|
||||
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() {
|
||||
myRobot.SetSafetyEnabled(false);
|
||||
@ -438,8 +370,8 @@ public:
|
||||
collectorSole2.Set(false);
|
||||
compressing = false;
|
||||
logMsg("Starting Teleop",1);
|
||||
while (IsEnabled() && IsOperatorControl()) {
|
||||
if(cur==50){
|
||||
while(IsEnabled() && IsOperatorControl()) {
|
||||
if(cur==50) {
|
||||
cur=0;
|
||||
WallLeft.Set(swap?1:0);
|
||||
BallRight.Set(swap?1:0);
|
||||
@ -448,31 +380,31 @@ public:
|
||||
swap=!swap;
|
||||
}
|
||||
throttle = (-Lstick.GetRawAxis(4)+1)/2;
|
||||
if(SmartDashboard::GetBoolean("Reverse Robot")){
|
||||
if(SmartDashboard::GetBoolean("Daniel Mode")) {
|
||||
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
||||
}else{
|
||||
} else {
|
||||
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
|
||||
}
|
||||
//Log things
|
||||
if (i % 200 == 0) {
|
||||
if(i % 200 == 0) {
|
||||
//logMsg(toString(compressor.GetPressureSwitchValue()),2);
|
||||
//logMsg("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();
|
||||
compressing = false;
|
||||
logMsg("Stopping the compressor",2);
|
||||
}
|
||||
if (i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
|
||||
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
|
||||
compressor.Start();
|
||||
compressing = true;
|
||||
logMsg("Starting the compressor... again",2);
|
||||
}
|
||||
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
|
||||
if (collectorExtended == false) {
|
||||
if(collectorExtended == false) {
|
||||
shooting = false;
|
||||
logMsg("Collector is NOT extended, not going to fire",17);
|
||||
}
|
||||
@ -488,34 +420,35 @@ public:
|
||||
logMsg("Stopping shooter motor",13);
|
||||
logMsg("Stopping collector motor",17);
|
||||
shootRobot(0, true);
|
||||
} else if (Lstick.GetRawButton(2)==1) {
|
||||
} else if(Lstick.GetRawButton(2)==1) {
|
||||
//Reverse the arm motors
|
||||
shooting = false;
|
||||
if (collectorExtended == false) {
|
||||
if(collectorExtended == false) {
|
||||
logMsg("Collector is not extended, not going to fire",17);
|
||||
}
|
||||
if (collectorExtended == true) {
|
||||
if(collectorExtended == true) {
|
||||
shootRobot(-DownSpeed, false);
|
||||
logMsg("Collector is extended, going to fire",17);
|
||||
} } else {
|
||||
}
|
||||
} else {
|
||||
shooting = false;
|
||||
//Stop all motors
|
||||
shootRobot(0, true);
|
||||
}
|
||||
if (Rstick.GetRawButton(9)==1) {
|
||||
if(Rstick.GetRawButton(9)==1) {
|
||||
collectorExtended = true;
|
||||
collectorSole1.Set(false);
|
||||
collectorSole2.Set(true);
|
||||
} else if (Rstick.GetRawButton(10)==1) {
|
||||
} else if(Rstick.GetRawButton(10)==1) {
|
||||
collectorExtended = false;
|
||||
collectorSole1.Set(true);
|
||||
collectorSole2.Set(false);
|
||||
}
|
||||
if (Lstick.GetRawButton(11)==1) {
|
||||
if(Lstick.GetRawButton(11)==1) {
|
||||
setMotorValue(6, 1, 1);
|
||||
} else if (Lstick.GetRawButton(12)==1) {
|
||||
} else if(Lstick.GetRawButton(12)==1) {
|
||||
setMotorValue(6, 1, 255);
|
||||
} else if (!shooting) {
|
||||
} else if(!shooting) {
|
||||
setMotorValue(6, 1, 0);
|
||||
}
|
||||
cur++;
|
||||
@ -525,4 +458,3 @@ public:
|
||||
}
|
||||
};
|
||||
START_ROBOT_CLASS(RobotDemo);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user