main.cpp

00001 /*
00002 ****** MAIN PROGRAM ******
00003 
00004 Please consider that although it is an embedded envrionment we are NOT creating a HARD-TIME real time system - delays can be dealt with
00005 
00006 Sensors are mapped on the global variable sensorsCheckSum,
00007 which multiplies the sensor number by itself to then decode,
00008  which sensors are off and which are on
00009 ie. if sensor rightright - sensorChecksum = 1*1 = 1
00010     if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
00011     ...
00012 */
00013 
00014 #include "mbed.h"
00015 #include "sensor_measure.h"
00016 #include "Motor_Driver.h"
00017 #include "shooter.h"
00018 
00019 #define PWM_PERIOD_US 10000
00020 
00021 typedef enum  mode {REGULAR,SQUARE} mode; //enumeration for different states
00022 DigitalOut led(LED_RED);
00023 DigitalOut ledMode(LED_GREEN);
00024 Serial HC06(PTE0,PTE1);
00025 
00026 void turnLeft ();
00027 void turnRight ();
00028 void measureSensors ();
00029 void printBluetooth();
00030 void shootFind ();
00031 
00032 
00033 Timer measureTimer; //Timer used for measurement
00034 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
00035 
00036 
00037 mode driveMode; //declaring the variable for the states
00038 int sensorsCheckSum; //varibale used for sensors mapping access
00039 int passedTime1,passedTime2;
00040 int oldValues[5] = {0};
00041 int cursor = 0, k = 0;
00042 static float normalSpeed = 0.15f;
00043 int countRightTurns = 0;
00044 int countLeftTurns = 0;
00045 
00046 
00047 int main() {
00048         
00049    //setup_counter(1000, 0);
00050    // float frequency = measure_frequency(CHANNEL_1);
00051     led = 1;//start LED with beginning of main program
00052     
00053     measureTimer.start();
00054     driveMode = REGULAR; //initialise drivemoder
00055     sensor_initialise(); // initialise sensor values
00056     wait(1); //give time to set up the system
00057     
00058     sensorTimer.start(); // start timer for sensors
00059     
00060     squareOut.period(0.01f);  // 0.01 second period
00061     squareOut.write(0.5);  // 50% duty cycle
00062 //    for (int b = 1; b <7;b++) {
00063 //                    shoot(0.4);
00064 //                }
00065     shoot (0);
00066 //    motors.setSteeringMode (NORMAL);
00067 //    motors.disableHardBraking();
00068 //    motors.forward ();
00069 //    
00070 //    motors.start (); 
00071 //    
00072 //    motors.setSpeed (normalSpeed);
00073 
00074     //motors.enableSlew();
00075  
00076     // HC06.printf("heello");
00077     int testOtherCases = 0; //used for control
00078     while(1){    
00079        
00080     // printBluetooth();
00081 //        while (HC06.getc() != 'r') {
00082 //            measureSensors();
00083 //            printBluetooth();
00084 //            wait(1);
00085 //        }
00086         
00087         
00088 
00089         measureSensors();
00090         printBluetooth();
00091         switch (sensorsCheckSum) {
00092             
00093             
00094             case 94:
00095             motors.setSpeed (normalSpeed);
00096             wait(0.15);
00097             break;
00098             
00099             case 104:
00100                       if ( countRightTurns == 1)
00101                         turnRight();
00102                     else {
00103                         motors.setSpeed (normalSpeed);
00104                         wait(0.15);
00105                         measureSensors(); 
00106                         
00107                         if (sensorsCheckSum == 104) { //confirm the turn
00108                             
00109                             wait(0.15);
00110                             measureSensors();
00111                             if (sensorsCheckSum == 104)
00112                                 turnRight ();
00113                         }
00114                     }
00115                 
00116             break;
00117             case 158:
00118                         motors.setSpeed (normalSpeed);
00119                         wait(0.15);
00120                         measureSensors(); 
00121                         
00122                         if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
00123                          
00124                             turnLeft();
00125                         }
00126                 break;
00127             case 168:
00128                 if ( countRightTurns == 1)
00129                  turnRight();
00130                 else {
00131                     motors.setSpeed (normalSpeed);
00132                     wait(0.15);
00133                     measureSensors(); 
00134                     
00135                     if (sensorsCheckSum == 168) { //confirm the turn
00136                         
00137                         wait(0.15);
00138                         measureSensors();
00139                         if (sensorsCheckSum == 168)
00140                             turnRight ();
00141                     }
00142                 }
00143             break;
00144             case 194:
00145                         motors.setSpeed (normalSpeed);
00146                         wait(0.15);
00147                         measureSensors(); 
00148                         
00149                         if (sensorsCheckSum == 194) { //confirm the turn
00150                             
00151                             
00152                             turnLeft();
00153                         }
00154                 break;
00155             default:
00156             testOtherCases =1;
00157             break;
00158         }
00159         if (testOtherCases == 1) {
00160             
00161             if (sensorsCheckSum < 96) {
00162                 motors.setSpeed (0.0);
00163                 motors.setLeftSpeed(normalSpeed);
00164                 wait(0.075);
00165             }
00166             else if (sensorsCheckSum > 96) {
00167                 motors.setSpeed (0.0);
00168                 motors.setRightSpeed(normalSpeed);
00169                 wait(0.1);
00170             }
00171             
00172         }   
00173         
00174         motors.setSpeed(0.0);
00175         wait(0.2);
00176         
00177         
00178     }
00179 } //int main
00180 
00181 void printBluetooth() { //for debugging
00182     HC06.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
00183     HC06.printf("LLD%i  LRD%i               rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
00184     //HC06.printf("%i  %i  %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
00185     //HC06.printf("%i      %i/n%i      %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);        
00186     //HC06.printf("%f                      %f",motor.getLeftSpeed(),motor.getRightSpeed());
00187     HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
00188     //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
00189 }
00190 
00191 
00192 
00193 void turnRight () {
00194     
00195     ledMode = 0; // put on LED to know we're in this state
00196     if (countRightTurns == 0 ) {
00197         motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
00198         wait(1);
00199         motors.setSteeringMode(PIVOT_CCW);
00200         motors.setSpeed (normalSpeed); //turn only the relevant wheel
00201         wait (1); 
00202     }
00203     else {
00204         motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
00205         wait(1);
00206         motors.setSteeringMode(PIVOT_CCW);
00207         motors.setSpeed (normalSpeed); //turn only the relevant wheel
00208         wait (0.8); 
00209     }
00210 
00211     int exit = 0; //used for exiting the function
00212     motors.setSpeed(0.0);
00213     wait(0.5);
00214     motors.setSteeringMode (NORMAL);
00215     wait(0.3);
00216     while (1) {   
00217         motors.setSpeed(0.0); //stop and check if the line is reached
00218         exit = 0;
00219         wait (0.4);
00220         for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
00221 
00222             if (measure(sensorArray[i]) == 0) {//if sensor is black 
00223                  exit ++;
00224             }
00225             if (exit > 2){ // two sensors are considered on line
00226                 motors.setSteeringMode(PIVOT_CCW);
00227                 motors.setSpeed(normalSpeed);
00228                 wait (0.3);
00229                 motors.setSpeed(0.0);
00230                 motors.setSteeringMode(NORMAL);
00231                 while (1) {
00232                     int checkSumLocal =0;
00233                     for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
00234 
00235                         if (measure(sensorArray[i]) == 0) 
00236                             checkSumLocal += i*i;
00237                     }     
00238                         printBluetooth();
00239                         if (checkSumLocal == 74) {
00240                             led = 0;
00241                             ledMode = 1; // turn off the LED
00242                             countRightTurns ++;
00243                             wait(1);
00244                             return;
00245                          }   
00246                          if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
00247                             motors.setSteeringMode(PIVOT_CCW);
00248                             motors.setSpeed(normalSpeed);
00249                             wait (0.6);
00250                             motors.setSpeed(0.0);
00251                             motors.setSteeringMode(NORMAL);
00252                          
00253                          }    
00254                         else if (checkSumLocal < 74) {
00255                             motors.setSpeed (0.0);
00256                             motors.setRightSpeed(normalSpeed);
00257                             wait(0.2);
00258                         }
00259                         else if (checkSumLocal > 74) {
00260                             motors.setSpeed (0.0);
00261                             motors.setLeftSpeed(normalSpeed);
00262                             wait(0.2);
00263                         }
00264                     motors.setSpeed(0.0);
00265                     wait(0.4);                      
00266                 }
00267 
00268             }        
00269 
00270         }
00271 //        motors.setSpeed(normalSpeed);
00272         motors.setRightSpeed (normalSpeed*1.7);
00273         motors.setLeftSpeed (normalSpeed);
00274         wait(0.2);
00275     }
00276 
00277 }
00278 
00279 void turnLeft () { 
00280 
00281     ledMode = 0; // put on LED to know we're in this state
00282     motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
00283     wait(1.1);
00284     motors.setSteeringMode(PIVOT_CW);
00285     motors.setSpeed (normalSpeed); //turn only the relevant wheel
00286     wait (1); 
00287     int exit = 0; //used for exiting the function
00288     motors.setSpeed(0.0);
00289     wait(0.5);
00290     motors.setSteeringMode (NORMAL);
00291     wait(0.3);
00292     while (1) {   
00293         motors.setSpeed(0.0); //stop and check if the line is reached
00294         exit = 0;
00295         wait (0.4);
00296         for (int i = 0; i < 4; i++) {
00297 
00298             if (measure(sensorArray[i]) == 0) {//if sensor is black 
00299                  exit ++;
00300             }
00301             if (exit > 2){ // two sensors are considered on line
00302                 motors.setSteeringMode(PIVOT_CW);
00303                 motors.setSpeed(normalSpeed);
00304                 wait (0.3);
00305                 motors.setSpeed(0.0);
00306                 motors.setSteeringMode(NORMAL);
00307                 while (1) {
00308                     int checkSumLocal =0;
00309                     for (int i = 0; i < 4; i++) {
00310 
00311                         if (measure(sensorArray[i]) == 1) 
00312                             checkSumLocal += (i+1)*(i+1);
00313                     }     
00314                         HC06.printf ("checkSumLocal is %i \n", checkSumLocal);
00315                         if (checkSumLocal == 20) {
00316                             led = 0;
00317                             ledMode = 1; // turn off the LED
00318                             countLeftTurns ++;
00319                             wait(1);
00320                             if (countLeftTurns == 1) {
00321                                 //shootFind ();
00322                             }    
00323                             return;
00324                          }   
00325                          if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
00326                             motors.setSteeringMode(PIVOT_CCW);
00327                             motors.setSpeed(normalSpeed);
00328                             wait (0.6);
00329                             motors.setSpeed(0.0);
00330                             motors.setSteeringMode(NORMAL);
00331                          
00332                          }    
00333                         else if (checkSumLocal > 20) {
00334                             motors.setSpeed (0.0);
00335                             motors.setRightSpeed(normalSpeed);
00336                             wait(0.2);
00337                         }
00338                         else if (checkSumLocal < 20) {
00339                             motors.setSpeed (0.0);
00340                             motors.setLeftSpeed(normalSpeed);
00341                             wait(0.2);
00342                         }
00343                     motors.setSpeed(0.0);
00344                     wait(0.4);                      
00345                 }
00346 
00347             }        
00348 
00349         }
00350 //        motors.setSpeed(normalSpeed);
00351         motors.setRightSpeed (normalSpeed);
00352         motors.setLeftSpeed (normalSpeed*1.5);
00353         wait(0.2);
00354     }
00355 }
00356 
00357 void shootFind () {
00358     motors.setSpeed(normalSpeed);
00359     wait (1);
00360     motors.setSteeringMode (PIVOT_CW);
00361     wait (0.8);
00362     motors.setSteeringMode (NORMAL);
00363 
00364     while (1) {
00365         motors.setSpeed(0.0);
00366         wait (0.3);
00367         for (int i = 8; i < 11 ; i++) {
00368             if (measure(sensorArray[i]) == 2){
00369                 motors.setSteeringMode (PIVOT_CW);
00370                 motors.setSpeed(normalSpeed);
00371                 wait (1.6);
00372                 motors.setSpeed(normalSpeed);
00373                 for (int b = 1; b <7;b++) {
00374                     shoot(0.1*(float)b);
00375                 }
00376                 shoot (0);
00377                 motors.setSteeringMode(NORMAL);
00378  //               while (1) {
00379 //                    motors.setSpeed(0.0);
00380 //                    wait (0.4);
00381 //                    for (int c =4 ;c <8;c++) {
00382 //                       if( measure(sensorArray[i]) == 0){
00383 //                            return;
00384 //                        }
00385 //                    }
00386 //                    motors.setSpeed(normalSpeed);
00387 //                    motors.setLeftSpeed(normalSpeed*1.3);
00388 //                    wait(0.2);
00389 //                }
00390                 return;
00391             }
00392                 
00393         }
00394         motors.setSpeed (normalSpeed);
00395         wait (0.5);
00396         
00397     }
00398 }
00399 
00400 void measureSensors () {
00401      sensorsCheckSum = 0; //zero it when first going into the routine
00402      int iterationNumber = NUMBER_SENSORS_REGULAR;
00403      if (driveMode == SQUARE) {
00404           iterationNumber += NUMBER_SENSORS_SQUARE;
00405      }
00406      for (int i = 0; i < iterationNumber;i++){
00407          //pc.printf("%i  iteration%i ",i,iterationNumber);
00408          if (measure(sensorArray[i]) == 1) {//if sensor is white 
00409             sensorsCheckSum += (i+1)*(i+1);
00410         }
00411      }
00412      if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
00413         oldValues[cursor] = sensorsCheckSum;
00414         cursor = (cursor+1)%5;
00415      }
00416      //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
00417 }