Robot.h

00001  class Robot {
00002     private:
00003     float static const distanceBetweenWheels = 0.19; //currently not used may be used later
00004     int static const numberOfSamples = 100; //number of samples the line voltage array will hold
00005     int static const numberOfSensors = 6; //how many sensors you want to use
00006     float AF; //attenuation factor just a number you want to minimize max speed by, obviously dont go higher than 1
00007     Wheel* leftWheel; //a pointer to the left wheel
00008     Wheel* rightWheel;
00009     lineSensor* sensorArray[numberOfSensors];
00010     PID2 controller;
00011     float lineVoltages[numberOfSamples];
00012     int lvIndex;
00013     int sensorNumber;
00014     int brakeC;
00015     
00016     Ticker updater;
00017     Timeout timeToStop;
00018     
00019     float Reflec;
00020     int endOfLineDetection;
00021     float RoboticAngularVelocity;
00022     
00023     char state;
00024     
00025     public:
00026     
00027     Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0006f) //this controller actually does nothing atm
00028     {
00029         endOfLineDetection = 0;
00030         updater.detach();
00031         timeToStop.detach();
00032         AF  = 0.7f; //change this value to change max speedzzzzzzzzzzzzzzzzz
00033         state = 'F';
00034         Reflec = 0;
00035           
00036         lvIndex = 0;
00037         sensorNumber = 0;
00038         sensorArray[0] = SA[0];
00039         sensorArray[1] = SA[1];
00040         sensorArray[2] = SA[2];
00041         sensorArray[3] = SA[3];
00042         sensorArray[4] = SA[4];
00043         sensorArray[5] = SA[5];
00044         leftWheel = LW;
00045         rightWheel= RW;
00046         
00047         lineVoltages[lvIndex] = 0.0f;
00048         
00049         controller.setInputLimits(-180.0f,180.0f);
00050         controller.setSetPoint(0.0f);
00051         //controller will output a value between +- max speed of wheels
00052         
00053         controller.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
00054     };
00055     
00056     float calculateTranslationalVelocity()
00057     {
00058         return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
00059     }
00060     
00061     //difference between angular velocities.
00062     void dW()
00063     {
00064         RoboticAngularVelocity = leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity();
00065     }
00066     
00067     float returnRoboticAngularVelocity()
00068     {
00069         return RoboticAngularVelocity;   
00070     }
00071     
00072     //attempts to modify the angular velocity of the buggy
00073     void adjustRbtAngularVelocity(float W)
00074     {
00075             //negative is a right turn
00076             if (W < 0)
00077             {
00078                 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
00079                 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()+W)*AF);
00080             }
00081             else
00082             {
00083                 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
00084                 rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()-W)*AF);
00085             }   
00086     }
00087     
00088     void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
00089         {
00090             switch (state)
00091             {
00092                 case 'T':
00093                     turn180();
00094                     break;
00095                 case 'S':
00096                     stopMovement();   
00097                     break;
00098                 default:
00099                     followLine();
00100                     break;
00101             }
00102         }
00103     
00104     void stopMovement(void)
00105     {
00106         leftWheel->adjustAngularVelocity(0.0f);
00107         rightWheel->adjustAngularVelocity(0.0f);
00108     }
00109     
00110     float returnLineVoltage()
00111     {
00112         return lineVoltages[lvIndex%numberOfSamples];
00113     }
00114     
00115     void rbtInit()
00116     {
00117     sensorArray[0]->sample();
00118     sensorArray[0]->calcLineVoltage();
00119     Reflec = sensorArray[0]->returnLineVoltage();
00120     updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
00121     }
00122     
00123     void spin()
00124     {
00125         rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
00126         leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel())*-0.3f);
00127     }
00128     
00129     void setState(char S)
00130     {
00131         state = S;
00132     }
00133     
00134     void followLine(void)
00135     {
00136         float ambientLight;
00137         float reading;
00138         ambientLight = sensorArray[sensorNumber]->calcLineVoltage();  
00139         sensorArray[sensorNumber]->sample();
00140         sensorArray[sensorNumber]->calcLineVoltage();
00141         reading = (sensorArray[sensorNumber]->returnLineVoltage()-ambientLight);
00142         switch (sensorNumber)
00143         {
00144         case 0:
00145             lineVoltages[lvIndex%numberOfSamples] = (reading * -130.0f); 
00146             if (reading <= 0.08f){endOfLineDetection++;}
00147             break;
00148         case 1:
00149             lineVoltages[lvIndex%numberOfSamples] += (reading * -52.5f);
00150             if (reading <= 0.08f) {endOfLineDetection++;}
00151             break;
00152         case 2:
00153             lineVoltages[lvIndex%numberOfSamples] += (reading * -16.25f);
00154             if (reading <= 0.08f) {endOfLineDetection++;}
00155             break;
00156         case 3:
00157             lineVoltages[lvIndex%numberOfSamples] += (reading * 16.25f);
00158             if (reading <= 0.08f) {endOfLineDetection++;}
00159             break;
00160         case 4: 
00161             lineVoltages[lvIndex%numberOfSamples] += (reading * 37.5f);
00162             if (reading <= 0.08f) {endOfLineDetection++;}
00163             break;
00164         case 5:
00165             lineVoltages[lvIndex%numberOfSamples] += (reading * 87.5f);
00166             if (reading <= 0.08f) {endOfLineDetection++;}
00167             break;
00168         }
00169                 
00170         sensorNumber ++;
00171         if (sensorNumber >= numberOfSensors)
00172         {
00173             sensorNumber = 0;
00174             if (endOfLineDetection < 5)
00175             {
00176                 AF = 0.625f;
00177                 adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
00178                 lvIndex++;
00179             }
00180             else
00181             {
00182                stopMovement(); 
00183             }
00184         endOfLineDetection = 0;
00185         }
00186     }
00187     
00188     void turn180()
00189     {
00190         updater.detach();
00191         rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
00192         leftWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*-0.3f);
00193         timeToStop.attach(callback(this, &Robot::reAttach),1.1f);
00194         state = 'S';
00195     }
00196     
00197     void reAttach()
00198     {
00199         updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
00200     }
00201     
00202     float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
00203     {
00204         if (var > prevMax) {var = prevMax;}
00205         if (var < prevMin) {var = prevMin;}
00206         return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
00207     }
00208 
00209 };
00210 
00211 /*
00212 Timeout timeToStop,
00213 
00214     
00215     void travelDistance(float d)//in metres
00216     {
00217         timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
00218     }
00219     
00220     void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
00221         {
00222             sensorArray[sensorNumber]->sample();
00223             
00224             if (sensorNumber < (numberOfSensors/2))
00225             {
00226                 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
00227             }
00228             else
00229             }
00230                 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
00231             }
00232             
00233             sensorNumber++;
00234             if (sensorNumber % numberOfSensors == 0)
00235             {
00236             sensorNumber = 0;
00237             controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
00238             adjustAngularVelocity(controller.compute());
00239             lvIndex++;
00240             }
00241         }
00242     
00243 */
00244 //lineVoltages[lvIndex] += 0.5f;
00245 
00246 
00247 
00248 /*if input value is greater than the maximum value the left wheel can go, go full right TURN
00249         if (W > leftWheel->returnMaxAngularVel()*AF)
00250         {
00251             leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
00252             rightWheel->adjustAngularVelocity(0); 
00253         }
00254         else if (W < (-1.0f*rightWheel->returnMaxAngularVel())) //if input value is less than the fastest the right wheel can go in reverse go full left TURN
00255         {
00256             rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
00257             leftWheel->adjustAngularVelocity(0); 
00258         }
00259         else if (W == 0)
00260         {
00261             rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
00262             leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);  
00263         }
00264         else
00265         {
00266 
00267 */