From 0b57a289ce1421bf42a40f4e5578001493a1d485 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Thu, 27 Feb 2014 04:02:49 +0000 Subject: [PATCH] Fixed up code after car voyage... Preparing to add ultrasonic code --- MyRobot.cpp | 318 +++++++++++++++++++++------------------------------- 1 file changed, 125 insertions(+), 193 deletions(-) diff --git a/MyRobot.cpp b/MyRobot.cpp index 0cc3737..de61d86 100644 --- a/MyRobot.cpp +++ b/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 string toString(numbertype a){ + template string toString(numbertype a) { //TODO stringstream ss; ss<3*sampleCount){ + } else if(cur3*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=potToDegrees(armPot.GetAverageVoltage())) { + } else { + if(i<=initalDriveTime*200) { + driveRobot(1.0f,0.0f); + //setMotorValue(6, 1, 1); + } else if(i>shooterDelay&&i=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=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); -