copy.cpp

00001 /***
00002 ********** MAIN PROGRAM **********
00003 
00004 Sensors are mapped on the global variable sensorsCheckSum,
00005 which multiplies the sensor number by itself to then decode,
00006  which sensors are off and which are on
00007 ie. if sensor rightright - sensorChecksum = 1*1 = 1
00008     if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
00009     ...
00010 ***/
00011 
00012 #include "mbed.h"
00013 #include "sensor_measure.h"
00014 #include "Motor_Driver.h"
00015 #include "shooter.h"
00016 
00017 #define PWM_PERIOD_US 10000
00018 
00019 typedef enum  mode {REGULAR,SQUARE} mode; //enumeration for different states
00020 DigitalOut led(LED_RED);
00021 DigitalOut ledMode(LED_GREEN);
00022 Serial HC06(PTE0,PTE1);
00023 
00024 void turnLeft ();
00025 void turnRight ();
00026 void measureSensors ();
00027 void printBluetooth();
00028 void shootFind ();
00029 
00030 
00031 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
00032 
00033 mode driveMode; //declaring the variable for the states
00034 int sensorsCheckSum; //varibale used for sensors mapping access
00035 int passedTime1,passedTime2;
00036 int cursor = 0, k = 0;
00037 static float normalSpeed = 0.15f;
00038 
00039 
00040 
00041 int main() {
00042     led = 1;//start LED with beginning of main program   
00043     measureTimer.start();
00044     driveMode = REGULAR; //initialise drivemoder
00045     sensor_initialise(); // initialise sensor values
00046     wait(1); //give time to set up the system
00047     
00048     sensorTimer.start(); // start timer for sensors
00049     
00050     motors.setSteeringMode (NORMAL);
00051     motors.disableHardBraking();
00052     motors.forward ();
00053     
00054     motors.start (); 
00055     
00056     motors.setSpeed (normalSpeed);
00057 
00058     motors.enableSlew();
00059     int testOtherCases = 0; //used for control
00060     while(1){       
00061         
00062         measureSensors();
00063         printBluetooth();
00064         switch (sensorsCheckSum) {    
00065             case 94: //continue straight
00066             motors.setSpeed (normalSpeed);
00067             break;
00068             case 104:
00069                 motors.setSpeed (normalSpeed); //move a little forward
00070                 measureSensors(); //measure again
00071                 if (sensorsCheckSum == 104 || sensorsCheckSum == 168) { //confirm the turn                
00072                     turnRight();
00073                 break;
00074             case 158:
00075                 motors.setSpeed (normalSpeed);
00076                 measureSensors(); 
00077                         
00078                 if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
00079                     turnLeft();
00080                 }
00081                 break;
00082             case 168:
00083                 motors.setSpeed (normalSpeed);
00084                 measureSensors(); 
00085                     
00086                 if (sensorsCheckSum == 168) { //confirm the turn
00087                     turnRight ();
00088                 }
00089                 
00090                 break;
00091             case 194:
00092                     motors.setSpeed (normalSpeed);
00093                     measureSensors(); 
00094                         
00095                     if (sensorsCheckSum == 194) { //confirm the turn      
00096                         turnLeft();
00097                     }
00098                     break;
00099             default:
00100                 testOtherCases =1;
00101             break;
00102         }
00103         if (testOtherCases == 1) {
00104             
00105             if (sensorsCheckSum < 96) { //turn left
00106                 motors.setSpeed (0.0);
00107                 motors.setLeftSpeed(normalSpeed);
00108                 wait(0.075);
00109             }
00110             else if (sensorsCheckSum > 96) { // turn right
00111                 motors.setSpeed (0.0);
00112                 motors.setRightSpeed(normalSpeed);
00113                 wait(0.1);
00114             }
00115             
00116         }   
00117         
00118         motors.setSpeed(0.0); //give time for the system to settle
00119         wait(0.2);
00120     }
00121 } //int main
00122 
00123 //print statements for debugging
00124 void printBluetooth() { /
00125     //HC06.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
00126     //HC06.printf("LLD%i  LRD%i               rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
00127     //HC06.printf("%i  %i  %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
00128     //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);        
00129     //HC06.printf("%f                      %f",motor.getLeftSpeed(),motor.getRightSpeed());
00130     //HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
00131     //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
00132 }
00133 
00134 
00135 
00136 void turnRight () {
00137     
00138     ledMode = 0; // put on LED to know we're in this state
00139     motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
00140     wait(1);
00141     motors.setSteeringMode(PIVOT_CW); //pivot for the turn
00142     motors.setSpeed (normalSpeed); 
00143     wait (1); 
00144     int exit = 0; //used for exiting the function
00145     motors.setSteeringMode (NORMAL);
00146     while (1) {   
00147         motors.setSpeed(0.0); //stop and check if the line is reached
00148         exit = 0;
00149         wait (0.4);
00150         for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
00151             if (measure(sensorArray[i]) == 0) {//if sensor is black 
00152                  exit ++;
00153             }
00154             if (exit > 2){ // if two sensors are black you're on the line
00155                 motors.setSteeringMode(PIVOT_CW); //rotate back to realign
00156                 motors.setSpeed(normalSpeed);
00157                 wait (0.3);
00158                 motors.setSteeringMode(NORMAL); //go back to normal
00159                 while (1) {
00160                     int checkSumLocal =0;
00161                     for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
00162                         if (measure(sensorArray[i]) == 0) 
00163                             checkSumLocal += i*i;
00164                     }     
00165                     if (checkSumLocal == 74) {
00166                         ledMode = 1; // turn off the LED to signify going out of this state
00167                         return;
00168                     }       
00169                     else if (checkSumLocal < 74) {
00170                          motors.setSpeed (0.0);
00171                          motors.setRightSpeed(normalSpeed);
00172                          wait(0.2);
00173                     }
00174                     else if (checkSumLocal > 74) {
00175                          motors.setSpeed (0.0);
00176                             motors.setLeftSpeed(normalSpeed);
00177                             wait(0.2);
00178                         }                 
00179                 }
00180 
00181             }        
00182 
00183         }
00184         motors.setRightSpeed (normalSpeed*1.7); //increase of the motors to bank left
00185         motors.setLeftSpeed (normalSpeed);
00186     }
00187 
00188 }
00189 
00190 void turnLeft () { 
00191 
00192     ledMode = 0; // put on LED to know we're in this state
00193     motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
00194     wait(1.1);
00195     motors.setSteeringMode(PIVOT_CCW);
00196     motors.setSpeed (normalSpeed); //turn only the relevant wheel
00197     wait (1); 
00198     int exit = 0; //used for exiting the function
00199     motors.setSteeringMode (NORMAL);
00200     while (1) {   
00201         motors.setSpeed(0.0); //stop and check if the line is reached
00202         exit = 0;
00203         for (int i = 0; i < 4; i++) {
00204             if (measure(sensorArray[i]) == 0) {//if sensor is black 
00205                 exit ++;
00206             }
00207             if (exit > 2){ // two sensors are considered on line
00208                 motors.setSteeringMode(PIVOT_CCW); //pivot
00209                 motors.setSpeed(normalSpeed);
00210                 wait (0.3);
00211                 motors.setSpeed(0.0);
00212                 motors.setSteeringMode(NORMAL);
00213                 while (1) {
00214                     int checkSumLocal =0;
00215                     for (int i = 0; i < 4; i++) {
00216                         if (measure(sensorArray[i]) == 1) 
00217                             checkSumLocal += (i+1)*(i+1);
00218                     }     
00219                     if (checkSumLocal == 20) {
00220                         led = 0;
00221                         ledMode = 1; // turn off the LED   
00222                         return;
00223                     }    
00224                     else if (checkSumLocal > 20) {
00225                          motors.setSpeed (0.0);
00226                          motors.setRightSpeed(normalSpeed);
00227                          wait(0.2);
00228                     }
00229                     else if (checkSumLocal < 20) {
00230                          motors.setSpeed (0.0);
00231                          motors.setLeftSpeed(normalSpeed);
00232                          wait(0.2);
00233                     }
00234                      
00235                 }
00236 
00237             }        
00238 
00239         }
00240         motors.setRightSpeed (normalSpeed);
00241         motors.setLeftSpeed (normalSpeed*1.5);
00242         wait(0.2);
00243     }
00244 }
00245 
00246 void shootFind () { //needs to be implemeted
00247 }
00248 
00249 void measureSensors () {
00250      sensorsCheckSum = 0; //zero it when first going into the routine
00251      int iterationNumber = NUMBER_SENSORS_REGULAR;
00252      if (driveMode == SQUARE) {
00253           iterationNumber += NUMBER_SENSORS_SQUARE;
00254      }
00255      for (int i = 0; i < iterationNumber;i++){
00256          if (measure(sensorArray[i]) == 1) {//if sensor is white 
00257             sensorsCheckSum += (i+1)*(i+1);
00258         }
00259      }
00260      if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
00261         oldValues[cursor] = sensorsCheckSum;
00262         cursor = (cursor+1)%5;
00263      }
00264 }