main.cpp

00001 #include <mbed.h>
00002 #include <math.h>
00003 #include "L298_H.h"
00004 #include "IRSensor_H.h"
00005 #include "ColourSensor_H.h"
00006 
00007 
00008 /*
00009 Team Design Project 3 Group 1
00010 Uploaded code V2.2
00011 
00012 -----PINOUT-----------------------
00013 
00014 Motor Left Control :
00015     - Motor Enable PWM :     PTA12
00016     - Backwards Dout :       PTA4
00017     - Forward Dout :         PTA5
00018 
00019 Motor Right Control :
00020     - Motor Enable PWM :     PTA13
00021     - Backwards Dout :       PTD5
00022     - Forward Dout :         PTD0
00023     
00024 IR Sensor Pinout :
00025     - Left Sensor :         PTC6
00026     - Middle Left Sensor :  PTC5
00027     - Middle Sensor :       PTC4
00028     - Middle Right Sensor : PTC3
00029     - Right Sensor :        PTC0
00030     
00031 Colour Sensor Pinout :
00032     - Colour Sensor 1:      PTB10
00033     - Colour Sensor 2:      PTB11
00034     - Solenoid :            PTE20
00035 
00036 UltraSonic Sensor Pinout :
00037     - Trigger Pin 1 :       PTE2
00038     - Echo pin 1 :          PTE3
00039     - Trigger Pin 2         PTE5
00040     - echopin2 :            PTE4
00041 
00042 Serial Sensor Pinout :
00043     - Esp Tx/Rx :           PTE22, PTE23
00044     - PC Tx/Rx :            USBTX, USBRX
00045     - psGroung :            PTC16
00046     - redLed :              PTC13
00047     - blueLed :             PTC12
00048     
00049 Switch and Debug :
00050     - Motor On :            PTE21         
00051     - myled :               LED_RED
00052 
00053 -----CODE BRIEF----------------------------
00054 The code puts together the class and code from the L298, IRsensor and ColourSensor headers to
00055 enable a the TDP3 Group1 robot to move, follow a line, detect a colour, detect an obstacle and
00056 communicate in Bluetooth.
00057 The code presents three main tickers which sample data from sensors from the Line Follower, 
00058 Colour Detection and Obstacle Detection. Further Timeouts and Interrupt are implemented.
00059   
00060 */
00061 
00062 
00063 
00064 //SERIAL Pinout===
00065 RawSerial ser(PTE22, PTE23); //TX, RX
00066 Serial pc(USBTX, USBRX);
00067 DigitalOut psGround(PTC16);
00068 DigitalOut redLed(PTC13);
00069 DigitalOut blueLed(PTC12);
00070 
00071 //========================
00072 
00073 //ULTRASONIC Pinout====
00074 DigitalOut trigpin(PTE2);
00075 DigitalIn echopin(PTE3);
00076 DigitalOut trigpin2(PTE5);
00077 DigitalIn echopin2(PTE4);
00078 //======================
00079 
00080 //Switch and Debug pinout=======
00081 DigitalIn motorOn(PTE21);       //switch which turns on or off the motors  
00082 DigitalOut myled(LED_RED);
00083 //==============================
00084 
00085 //IR init=======================
00086 Ticker IRsampling;              //ticker for the IR sampling
00087 //================================
00088 
00089 //COLOURSENSOR INIT======
00090 //initialize the Colour sensor object
00091 ColourSensor ColourSensor(PTB10, PTB11, PTE20);
00092 Timeout enableColour;
00093 Ticker detectColour;
00094 Timeout reactivateColour;
00095 Timeout solenoidBackOn;
00096 Timeout CheckColour;
00097 
00098 void ColourCheck();
00099 //=========================
00100 
00101 //US initialisation ===========
00102 Ticker US_Tic;
00103 Timer US_Timer;      //Timer used to record the time duration
00104 bool US_flag = false;  
00105 bool US_stop = false;   
00106 
00107 int getDis(DigitalIn echopin, DigitalOut trigPin);
00108 void US_inter();
00109 //==============================
00110 
00111 //SERIAL init===================
00112 char temp;
00113 bool serflag = false;
00114 bool enBle = false;
00115 bool serStop = false;
00116 char gain;
00117 float Kp = 0.0, Ki = 0.0, Kd = 0.0, noErSpeed = 0.0, turnSpeed = 0.0;
00118 
00119 void ReadRpi();
00120 void ReadSer(L298* controlLeft, L298* controlRight);
00121 //==============================
00122 
00123 void enableColourDet() {
00124     detectColour.attach(callback(&ColourSensor, &ColourSensor::readSensor), 0.05);   // Set call rate of colour detection function
00125     
00126 }
00127 
00128 
00129 
00130 
00131 
00132 //MAIN FUNCTION==================================================================
00133 int main(){
00134     
00135 
00136 
00137 //US +-------------
00138 float distance = 0;
00139 float distance2 = 0;
00140 //-----------------
00141 
00142 //SER +---------------------
00143 ser.format(8, SerialBase::None, 1);                 //Serial frame of 8 bit, no parity bit and 1 stop bit
00144 ser.baud(9600);                                     //Serial baudrate set at 9600
00145 psGround = false;
00146 redLed = false;
00147 blueLed = false;
00148 //--------------------------
00149 
00150 bool toggleIR;
00151 
00152 
00153 
00154 //initialize the IRsensor object, the different parameters can be tuned at will (Kp,Ki,Kd,noErSpeed,turnSpeed)
00155 IRSensor IRSensor(PTC6, PTC5, PTC4, PTC3, PTC0, 0.9, 0, 2.3 ,0.16,0.1);//0.58,0,2.6,0.16,0.08 || 0.65, 0.0 , 3.0, 0.15, 0.11
00156 
00157 //initialise the class twice, one controls the left part and the other the right
00158 L298 controlLeft(PTA13,PTD5,PTD0);
00159 L298 controlRight(PTA12,PTA4,PTA5);
00160 
00161 
00162 //Interrupt Routines======================
00163 enableColour.attach(&enableColourDet, 1.0);
00164 IRsampling.attach(callback(&IRSensor, &IRSensor::Sample), 0.008);
00165 ser.attach(&ReadRpi,Serial::RxIrq);
00166 US_Tic.attach(&US_inter,0.3);
00167 //========================================
00168 
00169 
00170 //Initialise the variables changed by the phone
00171 Kp = IRSensor.m_Kp; 
00172 Ki = IRSensor.m_Ki;
00173 Kd = IRSensor.m_Kd;
00174 noErSpeed = IRSensor.m_noErSpeed;
00175 turnSpeed = IRSensor.m_turnSpeed;
00176 
00177 //motor init
00178 controlLeft.SetSpeed(0);
00179 controlRight.SetSpeed(0);
00180 
00181 myled = 1;
00182 gain = 0;
00183 
00184 
00185 while(1){
00186     
00187     //enable and disable the IR process when in bluetooth mode
00188     if(!enBle ||!serStop){
00189     if(motorOn && !US_stop){
00190         
00191         //IR sensors==========================================================================
00192         if(toggleIR != IRSensor.m_toggle){
00193         //waits for an interrupt to occur, then performs all the calculations
00194             
00195             IRSensor.WeightPID();
00196             IRSensor.CalculatePID();
00197             IRSensor.MotorControl();
00198         
00199             if(IRSensor.m_dirL != IRSensor.m_prevDirL) controlLeft.SetDirection(IRSensor.m_dirL);
00200             if(IRSensor.m_dirR != IRSensor.m_prevDirR) controlRight.SetDirection(IRSensor.m_dirR);
00201     
00202             controlLeft.SetSpeed(fabs(IRSensor.m_speedL));
00203             controlRight.SetSpeed(fabs(IRSensor.m_speedR));
00204     
00205             IRSensor.m_prevDirL = IRSensor.m_dirL;
00206             IRSensor.m_prevDirR = IRSensor.m_dirR;
00207         
00208         
00209             //printf("rightSpeed : %f, leftSpeed : %f \r", IRSensor.m_speedL, IRSensor.m_speedR);    
00210             toggleIR = IRSensor.m_toggle;
00211             }
00212         
00213         //=========================================================================================
00214         }
00215     }
00216     
00217     else if(!motorOn || US_stop || serStop){
00218         //turns off the the motors
00219         controlLeft.Stop();
00220         controlRight.Stop();    
00221         } 
00222     
00223         
00224         
00225     
00226     //Serial Communication =============================================================================
00227     if(serflag == 1){
00228     //read serial and assign Kp values        
00229         
00230         ReadSer(&controlLeft, &controlRight);
00231         
00232         //make the phone values equal to the one used by the PID
00233         IRSensor.m_Kp = Kp;
00234         IRSensor.m_Ki = Ki;
00235         IRSensor.m_Kd = Kd;
00236         IRSensor.m_noErSpeed = noErSpeed;
00237         IRSensor.m_turnSpeed = turnSpeed;
00238         
00239         serflag = 0;
00240         myled = !myled;   
00241         }
00242     //===================================================================================================
00243     
00244     
00245     //COLOUR SENSOR======================================================================================
00246         
00247     if (ColourSensor.toggleConst && ColourSensor.toggleA) {
00248         // Call convert to convert sensor logic
00249         ColourSensor.convert();
00250         
00251         if(!ColourSensor.diskHave){
00252             blueLed = 0;
00253             redLed = 0;}
00254             
00255         if(ColourSensor.newDetection){
00256              //New Detection found, detach the interrupt, trigger a timeout to check the colour
00257              detectColour.detach();
00258              CheckColour.attach(ColourCheck, 0.5); 
00259              }
00260         ColourSensor.toggleA = 0;
00261     }
00262         
00263     if (ColourSensor.toggleConst && ColourSensor.toggleB && ColourSensor.toggleC) {
00264         // Process colour sensor data
00265         ColourSensor.process();
00266             
00267         //Convert the colour of the disk to the correction the robot has to make
00268         if(ColourSensor.diskColour && ColourSensor.diskHave){
00269                 IRSensor.m_color = -5;
00270                 blueLed = 1;
00271                 redLed = 0;
00272                 }
00273         else if(!ColourSensor.diskColour && ColourSensor.diskHave){
00274                 IRSensor.m_color = 5;
00275                 blueLed = 0;
00276                 redLed = 1;
00277                 }
00278         
00279         reactivateColour.attach(callback(&ColourSensor, &ColourSensor::makeColourActive), 2.0);
00280             
00281         ColourSensor.toggleB = 0;
00282     }
00283     //==========================================================================================
00284        
00285     
00286     //Ultrasonic Sensor =============================================================================
00287     if(US_flag){
00288         //disable irq to ensure correct readings
00289         //get distance of both sensors and stop the robot if and object is at less than 20cm
00290         __disable_irq(); 
00291     
00292         distance = 0;
00293         distance = getDis(echopin, trigpin);
00294     
00295         distance2 = 0;
00296         distance = getDis(echopin2, trigpin2);
00297     
00298         __enable_irq(); 
00299         
00300         //check distance
00301         if (((distance < 20.0) && (distance > 0)) || ((distance2 < 20.0) && (distance2 > 0)) ) { 
00302             US_stop = 1;
00303             myled = 1;
00304             }
00305         else {
00306             US_stop = 0;
00307             myled = 0;
00308             }
00309         US_flag = 0;
00310         
00311         }
00312        
00313     }
00314 }
00315 
00316 
00317 
00318 
00319 
00320 
00321 void ReadRpi(){
00322 //attached to interrupt
00323 //reads and store the char sent on Rx in temp
00324     
00325     if(ser.readable()) temp = ser.getc();
00326     serflag =  1;
00327 }
00328 
00329 
00330 
00331 void ReadSer(L298* controlLeft, L298* controlRight){
00332     //reads the information in temp 
00333     //if temp is either p,i or d, it assigns the value to gain
00334     //else if temp is + or -, it either increases or decreases the chosen parameter
00335         
00336     if(temp == 'p' || temp == 'i' || temp == 'd' || temp == 's' || temp == 'e' || temp == 'x' || temp == 'z' || temp == 'b' || temp == 'n'){
00337         gain = temp;
00338         }
00339     
00340     switch(gain){
00341         case 'p': 
00342             if(temp == '+' )Kp += 0.01;                  
00343             else if(temp == '-' && Kp > 0) Kp -= 0.01;     
00344             break;
00345         case 'i':
00346             if(temp == '+') Ki += 0.001;
00347             else if(temp == '-' && Ki > 0) Ki -= 0.0001;    
00348             break;
00349         case 'd':
00350             if(temp == '+') Kd += 0.1;   
00351             else if(temp == '-' && Kd > 0) Kd -= 0.1;    
00352             break;
00353         case 's':
00354             if(temp == '+') noErSpeed += 0.1;   
00355             else if(temp == '-' && noErSpeed > 0) noErSpeed -= 0.1; 
00356             break;
00357         case 'v':
00358             if(temp == '+') turnSpeed += 0.1;   
00359             else if(temp == '-' && turnSpeed > 0) turnSpeed -= 0.1; 
00360             break; 
00361         case 'x': //stop
00362             serStop = true;
00363             break;
00364         case 'z': 
00365             serStop = false;
00366             break;
00367         case 'b':
00368             enBle = true;
00369             break;
00370         case 'n':
00371             enBle = false;
00372             break;
00373                     
00374         default:
00375             break;
00376             }
00377         
00378     
00379     if (enBle == true){
00380         //by inputing b, the robot will become controlled by the phone user
00381         //inputting a direction will apply this direction for until a new direction is given
00382         
00383         switch(temp){
00384             case'8': //forward
00385             
00386                 controlLeft->SetDirection(1);
00387                 controlRight->SetDirection(1);
00388                 controlLeft->SetSpeed(0.2);
00389                 controlRight->SetSpeed(0.2);
00390                 
00391                 break; 
00392             case'4':  //left
00393             
00394                 controlLeft->SetDirection(0);
00395                 controlRight->SetDirection(1);
00396                 controlLeft->SetSpeed(0.2);
00397                 controlRight->SetSpeed(0.2);
00398                 
00399                 break; 
00400             case'2': //backwards
00401                 
00402                 controlLeft->SetDirection(0);
00403                 controlRight->SetDirection(0);
00404                 controlLeft->SetSpeed(0.2);
00405                 controlRight->SetSpeed(0.2);
00406             
00407                 break; 
00408             case'6': //right
00409             
00410                 controlLeft->SetDirection(1);
00411                 controlRight->SetDirection(0);
00412                 controlLeft->SetSpeed(0.2);
00413                 controlRight->SetSpeed(0.2);
00414                 
00415                 break; 
00416             default: break;
00417             
00418         }
00419     }
00420         
00421     serflag = 0;
00422 }
00423 
00424 
00425 
00426 int getDis (DigitalIn echopin, DigitalOut trigPin) {
00427     // gets the distance from the ultrasonic sensor
00428     //set the trigger pin on one for 10 usec and then process to get the data
00429     Timer t;
00430     float distance;
00431     float duration; 
00432     
00433     //sends a trigger of 10 us 
00434     trigPin = 0; 
00435     wait_us(2);
00436     trigPin = 1;
00437     wait_us(10);
00438     trigPin = 0; 
00439 
00440     //measure the time taken from the echopin to stay high
00441     while (!echopin);         // read the duration time
00442     t.start();
00443     while (echopin){if(t.read() >= 10000){break;}}
00444     t.stop();
00445     
00446     duration = t.read_us(); 
00447     //pc.printf("1 : %f ", duration);
00448     distance = (duration/2) / 29.1;  
00449     t.reset();
00450     
00451     //printf("distance: %d\n",distance); //read the distance
00452     return distance;
00453 }
00454 
00455 
00456 void US_inter(){
00457     //trigger a flag to start the US routine
00458     US_flag=1;
00459     }
00460     
00461 void ColourCheck(){
00462     
00463     ColourSensor.sensorOneCheck = ColourSensor.inOne.read();
00464     ColourSensor.sensorTwoCheck = ColourSensor.inTwo.read();
00465     
00466     bool sensorColourCheck = ColourSensor.sensorOneCheck & ColourSensor.sensorTwoCheck;
00467     
00468    if( ColourSensor.sensorColour == sensorColourCheck)ColourSensor.toggleC = 1;
00469    else ColourSensor.toggleB = 0;
00470    detectColour.attach(callback(&ColourSensor, &ColourSensor::readSensor), 0.05);   // Set call rate of colour detection function
00471    
00472    }
00473 
00474 
00475