obstacle.cpp

00001 /**
00002 @file obstacle.cpp
00003 @brief Program Implementation
00004 */
00005 
00006 #include "mbed.h"
00007 #include "SRF02.h"
00008 #include "N5110.h"
00009 #include "beep.h"
00010 #include "obstacle.h"
00011 
00012 
00013 //////////////////////////PART 1 - SETTING UP THE MEASUREMENTS//////////////////////////////
00014 
00015 int timerFlag=0;
00016 void timerExpired()
00017 {
00018     timerFlag =1;
00019 }
00020 
00021 void error(int code)
00022 {
00023     while(1) {
00024         leds = 0;
00025         wait(0.25);
00026         leds = code;
00027         wait(0.25);
00028     }
00029 }
00030 
00031 void measureExpired()
00032 {
00033     measureFlag =1;
00034 }
00035 
00036 
00037 /*
00038 void initTMP102()
00039 {
00040     tmp102.frequency(400000); // set bus speed to 400 kHz
00041     int ack; // used to store acknowledgement bit
00042     char data[2]; // array for data
00043     char reg = CONFIG_REG; // register address
00044 //////// Read current status of configuration register ///////
00045 
00046     ack = tmp102.write(TMP102_W_ADD,&reg,1); // send the slave write address and the configuration register address
00047     if (ack)
00048         error(1); // if we don't receive acknowledgement, flash error message
00049     ack = tmp102.read(TMP102_R_ADD,data,2); // read default 2 bytes from configuration register and store in buffer
00050     if (ack)
00051         error(2); // if we don't receive acknowledgement, flash error message
00052 ///////// Configure the register //////////
00053 // set conversion rate to 1 Hz
00054     data[1] |= (1 << 6); // set bit 6
00055     data[1] &= ~(1 << 7); // clear bit 7
00056 
00057 //////// Send the configured register to the slave ////////////
00058 
00059     ack = tmp102.write(TMP102_W_ADD,&reg,1); // send the slave write address and the configuration register address
00060     if (ack)
00061         error(3); // if we don't receive acknowledgement, flash error message
00062     ack = tmp102.write(TMP102_W_ADD,data,2); // send 2 data bytes to the configuration register
00063     if (ack)
00064         error(4); // if we don't receive acknowledgement, flash error message
00065 
00066 }
00067 
00068 float getTemperature()
00069 {
00070     int ack; // used to store acknowledgement bit
00071     char data[2]; // array for data
00072     char reg = TEMP_REG; // temperature register address
00073     ack = tmp102.write(TMP102_W_ADD,&reg,1); // send temperature register address
00074     if (ack)
00075         error(5); // if we don't receive acknowledgement, flash error message
00076     ack = tmp102.read(TMP102_R_ADD,data,2); // read 2 bytes from temperature register and store in array
00077     if (ack)
00078         error(6); // if we don't receive acknowledgement, flash error message
00079     temperature = (data[0] << 4) | (data[1] >> 4);
00080     return temperature*0.0625;
00081 }
00082 
00083 float calcSoundSpeed(float temperature)
00084 {
00085     return soundSpeed = temperature*0.6 + 331.3;
00086 }
00087 
00088 float calcSoundTimeDelay (int distance, float soundSpeed)
00089 {   distance =srf.getDistanceCm();
00090     return soundTimeDelay = distance/soundSpeed;
00091 }
00092 */
00093 
00094 
00095 ///////////////////PART 2 - Distance Upadate and Speed //////////////////////
00096 
00097 
00098 void updateExpired()
00099 {
00100     updateFlag=1;    ///
00101 }
00102 
00103 void update_distance_new()
00104 {
00105     lastDistance = newDistance; ///updates the lastDistance variable by storing into it the previous distance measurement
00106     newDistance = srf.getDistanceCm();///updates the newDistance variable by adding to it the new distance measurement
00107 }
00108 
00109 
00110 ///////////////////PART 3 - Distance Range Alert//////////////////////////////
00111 
00112 void rangeExpired()
00113 {
00114     rangeFlag =1;
00115 }
00116 
00117 struct State {
00118     float bOutput; /// buzzer output value
00119     int nextState[5]; /// array of next states
00120 };
00121 typedef const struct State STyp;
00122 
00123 STyp fsm[5] = {
00124     {8000,{0,1,2,3,4}}, // State 0 - object at less than 20 cm away
00125     {2000,{0,1,2,3,4}}, // 1 - object in the 20-65cm range from the person
00126     {1000,{0,1,2,3,4}}, // 2 - object in the 65-110cm range from the person
00127     {500,{0,1,2,3,4}}, // 3 - object in the 110-155cm range from the person
00128     {200,{0,1,2,3,4}}, // 4 - object in the 155-200cm range from the person
00129 
00130 };
00131 
00132 void updateState()
00133 {
00134 
00135     distance = srf.getDistanceCm();
00136     if(distance<=20) rangeState=0;
00137     else if((distance>20)&&(distance<=65))rangeState=1;
00138     else if ((distance>65)&&(distance<=110))rangeState=2;
00139     else if ((distance>110)&&(distance<=155))rangeState=3;
00140     else if ((distance>155)&&(distance<=200))rangeState=4;
00141 
00142 
00143     currentState = fsm[currentState].nextState[rangeState];
00144 }
00145 
00146 ///////////////////PART 4 - Beam Pattern ///////////////////////////////////
00147 
00148 double angleArray [5]= {0,10,20,30,40}; /*!<array of angles from the center of the sensor*/
00149 
00150 
00151 void beamExpired()
00152 {
00153     beamFlag=1;
00154 }
00155 
00156 double convertDegrees(double angleDegrees)
00157 {
00158      ///uses equivalence of 2pi and 180 degrees
00159     return angleDegrees*3.16/180;     
00160 }
00161 
00162 void drawSemiCircle()
00163 {
00164 //draws a semi circle on the screen of radius equal to the distance from the object
00165     int radius = srf.getDistanceCm(); /// int radius = ((30*srf.getDistanceCm())/20); other possibility
00166     lcd.drawCircle(WIDTH/2,1,radius,0);  /// x,y,radius,transparent with outline
00167     lcd.drawLine(WIDTH/2,1,WIDTH/2,radius,2); ///draws one diameter
00168     lcd.refresh();
00169 }
00170 
00171 void drawBeamLines ()
00172 {
00173 ///draws a beam line for each calcualted value
00174     lcd.drawLine(WIDTH/2,1,WIDTH/2+l,h,2);
00175     lcd.drawLine(WIDTH/2,1,WIDTH/2-l,h,2);
00176     lcd.refresh();
00177 }
00178 
00179 double lat(double a, double d)
00180 {
00181     return (tan(a) *d);
00182 }
00183 
00184 double hyp(double a, double d)
00185 {
00186     return ((tan(a) *d)/sin(a));
00187 }
00188 
00189 /////////////////////////PART 5 - the Wall Follower Simulation //////////////////////
00190 
00191 void wallExpired()
00192 {
00193     wallFlag=1;
00194 }
00195 
00196 void moveForward()
00197 {
00198 /// sets pixel to move forward
00199 
00200     for(int k=0; k<=22; k++) {
00201         lcd.setPixel(16,k);  /// (x,y) starting point of the pixel - here fixed x coordinate - vertical movement
00202         wait(0.2);  ///can be considered as the "speed of the movement" because it defines the rate if change of pixel - delay between two k values
00203         pix++;  ///increments the pix vlaue for each increase in k
00204         lcd.refresh();
00205     }
00206 }
00207 
00208 void turnLeftForward_one()
00209 {
00210 ///sets the pixel to turn left forward
00211 
00212     for(int l=0; l<=64; l++) {
00213         lcd.setPixel(l+16,22);  /// (x,22) starting point of the pixel - here fixed y coordinate - horizontal movement
00214         wait(0.2);
00215         pix=pix+1; ///increments the pix vlaue for each increase in l
00216         lcd.refresh();
00217     }
00218 
00219 }
00220 
00221 void turnLeftForward_two()
00222 {
00223 ///sets the pixel to turn right forward -one
00224 
00225     for (int m=0; m<=16; m++) {
00226         lcd.setPixel(80,m+22);  /// (80,y) starting point of the pixel - here fixed x coordinate - vertical movement
00227         wait(0.2);
00228         pix=pix+1; ///increments the pix vlaue for each increase in m
00229         lcd.refresh();
00230     }
00231 
00232 }
00233 
00234 void turnRightForward()
00235 {
00236 ///sets the pixel to turn right forward -two
00237 
00238     for(int n=0; n<=78; n++) {
00239         lcd.setPixel(80-n,38);  /// (x,38) starting point of the pixel - here fixed y coordinate - horizontal movement- x=80-n because counting down from the end x=80 to he beginning x=2 of the screen
00240         wait(0.2);
00241         pix=pix+1; ///increments the pix vlaue for each increase in n
00242         lcd.refresh();
00243     }
00244     //lcd.refresh();
00245 }
00246 
00247 
00248 ///intial beep zero move forward
00249 void beep_zero()
00250 {
00251     int sound_zero = 200; /// local varaible integer gives specific frequency value for the buzzer
00252     float time_zero = 0.22; /// local varaible integer gives specific time for the buzzer
00253     buzzer.beep(sound_zero,time_zero);
00254 
00255 }
00256 
00257 ///turns on beep one Turn Left
00258 void beep_one()
00259 {
00260     int sound_one = 600;
00261     float time_one = 0.2;
00262     buzzer.beep(sound_one,time_one);
00263 }
00264 
00265 /// turns on beep two Trun Left and move Forward
00266 void beep_two()
00267 {
00268     int sound_two = 800;
00269     float time_two = 0.23;
00270     buzzer.beep(sound_two,time_two);
00271 }
00272 
00273 //turns on beep three Turn Right
00274 void beep_three()
00275 {
00276     int sound_three = 1600;
00277     float time_three = 0.2;
00278     buzzer.beep(sound_three,time_three);
00279 }
00280 
00281 /// turns on beep four Turn Right and move Forward
00282 void beep_four()
00283 {
00284     int sound_four = 2000;
00285     float time_four = 0.14;
00286     buzzer.beep(sound_four,time_four);
00287 }
00288 
00289 /// turns on beep five Stop Dead End
00290 void beep_five()
00291 {
00292     int sound_five = 2600;
00293     float time_five = 0.6;
00294     buzzer.beep(sound_five,time_five);
00295 }
00296 
00297 void toggle_red()
00298 {
00299     red=!red; ///toggle red led
00300 }
00301 void toggle_yellow()
00302 {
00303     yellow=!yellow;  ///toggles yellow led
00304 }
00305 void toggle_green()
00306 {
00307     green=!green;  ///toggles green led
00308 }
00309 
00310 
00311 
00312 void move()
00313 {
00314     if (pix<=22) {
00315         lcd.printString(" Forward ",32,0);
00316         beep_ticker_zero.attach(&beep_zero, 0.61); //move forward
00317         toggle_green_ticker.attach(&toggle_green, 0.61);
00318         moveForward();
00319     }
00320 
00321     beep_ticker_zero.detach();
00322     beep_ticker_one.attach(&beep_one, 0.2);//turn left
00323     lcd.printString(" T Left ",32,0);
00324     toggle_green_ticker.detach();
00325     toggle_red_ticker.attach(&toggle_red,0.1);
00326     wait(0.5);
00327     beep_ticker_one.detach();
00328    
00329 
00330     if (22<pix && pix<86) {
00331         toggle_red_ticker.detach();
00332         lcd.printString(" Forward ",32,0);
00333         beep_ticker_two.attach(&beep_two, 0.66); //turn left and move forward
00334         toggle_green_ticker.attach(&toggle_green, 0.66);
00335         turnLeftForward_one();
00336     }
00337 
00338     beep_ticker_two.detach();
00339     beep_ticker_three.attach(&beep_three,0.2);//turn right
00340     lcd.printString(" T Right ",32,0);
00341     toggle_green_ticker.detach();
00342     toggle_red_ticker.attach(&toggle_red,0.1);
00343     wait(0.5);
00344     beep_ticker_three.detach();
00345     
00346 
00347     if(86<pix && pix<102) {
00348         toggle_red_ticker.detach();
00349         lcd.printString(" Forward ",32,0);
00350         toggle_green_ticker.attach(&toggle_green, 0.76);
00351         beep_ticker_four.attach(&beep_four, 0.76);//turn right and move forward
00352         turnLeftForward_two();
00353     }
00354 
00355     beep_ticker_four.detach();
00356     beep_ticker_three.attach(&beep_three,0.2);//turn right
00357     lcd.printString(" T Right ",32,0);
00358     toggle_red_ticker.attach(&toggle_red,0.1);
00359     wait(0.5);
00360     beep_ticker_three.detach();
00361    
00362 
00363     if (102<pix) {
00364         toggle_red_ticker.detach();
00365         beep_ticker_four.attach(&beep_four,0.8);///turn right and move forward- function called every 0.8s
00366         toggle_green_ticker.attach(&toggle_green, 0.76); /// toggle green led function called every 0.76s
00367         lcd.printString(" Forward ",32,0);
00368         turnRightForward();
00369     }
00370 
00371     beep_ticker_four.detach();
00372     toggle_green_ticker.detach();
00373     lcd.printString("  STOP!! ",32,0);
00374     toggle_red_ticker.attach(&toggle_red,0.1);
00375     beep_ticker_five.attach(&beep_five,0.8);//stop dead end
00376 }
00377 
00378 void drawMaze()
00379 {
00380     lcd.drawRect(0,0,12,25,1);  /// top left filled rectangle-green on report
00381     lcd.drawRect(20,0,10,20,1); /// top middle filled rectangle-orange on report
00382     lcd.drawRect(30,10,52,10,1); /// top right filled rectangle-pink on report
00383     lcd.drawRect(0,25,76,10,1); /// middle filled rectangle-purple on report
00384     lcd.drawRect(0,40,80,1,1); /// bottom filled rectangle-red on report
00385     lcd.drawRect(0,36,1,4,1); /// dead end small filled rectangle-black on report
00386     lcd.refresh();
00387 }
00388 
00389 ///////////////////////////////Transitions/User interface (button) selection and event order elements//////////////////////////////////
00390 
00391 void buttonPressed ()
00392 {
00393     buttonFlag++;  ///increments and keepd the value of the buttonFlag counter for every button press
00394     if ( buttonFlag > 4) {  /// sets the limit value of the buttonFlag before going back to zero => five different flag states to be used for the five different modules
00395         buttonFlag = 0;
00396     }
00397 }
00398 
00399 
00400 void manageScreen()
00401 {
00402     if ( buttonFlag== 0) {
00403         measureFlag = !measureFlag; /// toggles measureFlag - if the flag is intially low then it goes high for buttonFlag = 0 - first screen view
00404     }
00405     if ( buttonFlag == 1) {
00406         updateFlag = !updateFlag; /// toggles updateFlag - if the flag is intially low then it goes high for buttonFlag = 1 - first button press - second screen view
00407     }
00408     if ( buttonFlag == 2) {
00409 
00410         rangeFlag = !rangeFlag;  /// toggles rangeFlag - if the flag is intially low then it goes high for buttonFlag = 2 - second button press - third screen view
00411     }
00412     if ( buttonFlag == 3) {
00413         beamFlag = !beamFlag;  /// toggles beamFlag - if the flag is intially low then it goes high for buttonFlag = 3 - third button press - fourth screen view
00414     }
00415     if ( buttonFlag == 4) {
00416         wallFlag = !wallFlag;  /// toggles wallFlag - if the flag is intially low then it goes high for buttonFlag = 4 - fourth button press - fifth screen view
00417     }
00418 
00419     if(measureFlag == 1) {
00420         measure_ticker.attach(&update_distance_new,1.0);
00421         //  temperature_ticker.attach(&getTemperature,1.0);
00422     } else {
00423         measure_ticker.detach();
00424     }
00425 
00426     if(updateFlag == 1) {
00427 
00428         update_ticker.attach(&updateExpired,2.0);
00429 
00430     } else {
00431         update_ticker.detach();     // detach ticker
00432     }
00433 
00434     if(rangeFlag == 1) {
00435         range_ticker.attach(&rangeExpired,3.0);
00436     } else {
00437         range_ticker.detach();
00438     }
00439 
00440     if(beamFlag == 1) {
00441         beam_ticker.attach(&beamExpired,1.0);
00442     } else {
00443         beam_ticker.detach();
00444     }
00445 
00446     if(wallFlag == 1) {
00447         wall_ticker.attach(&wallExpired, 4.0);
00448     } else {
00449         wall_ticker.detach();
00450     }
00451 }
00452 
00453 
00454 
00455 
00456 
00457 int main ()
00458 {
00459     lcd.init(); //initializes the LCD display
00460 ///  initTMP102();
00461 
00462     lcd.normalMode();      /// normal colour mode
00463     lcd.setBrightness(0.5); /// put LED backlight on 50%
00464     button.rise(&buttonPressed ); /// indicates that the button has been pressed. button flag rises when button pressed
00465 
00466     measure_ticker.attach(&measureExpired,1.0);
00467     update_ticker.attach(&updateExpired,2.0);
00468     beam_ticker.attach(&beamExpired,3.0);
00469     //  update_ticker_two.attach(&update_distance_new,2.0);
00470     range_ticker.attach(&rangeExpired,4.0);
00471     wall_ticker.attach(&wallExpired, 5.0);
00472 
00473 
00474     while(1) {
00475         // distance = srf.getDistanceCm();
00476 
00477         manageScreen();
00478         if (timerFlag) {
00479             timerFlag=0;   ///reset flag
00480 
00481 
00482             if(measureFlag) {
00483                 lcd.clear();
00484                 measureFlag =0;  ///reset flag
00485 
00486                 char buffer[14];
00487 
00488                 distance = srf.getDistanceCm();  /// same idea with floats
00489                 float length = sprintf(buffer,"D = %.2f cm",distance);
00490                 if (length <= 14)
00491                     lcd.printString(buffer,0,4);
00492 
00493             /*    temperature = getTemperature();
00494                      int lengthT = sprintf(buffer,"T = %.2f C",temperature); /// print formatted data to buffer
00495                      if (lengthT <= 14)  /// if string will fit on display
00496                          lcd.printString(buffer,0,5);
00497                
00498                 soundSpeed = calcSoundSpeed(temperature);
00499                 soundTimeDelay = calcSoundTimeDelay(distance, soundSpeed);
00500                 */
00501             }
00502             lcd.refresh();
00503 
00504 
00505             if(updateFlag) {
00506                 lcd.clear();
00507                 updateFlag=0;  ///reset flag
00508                 char bufferOne[14]; ///character array that stores the values of the newDistance variable
00509                 char bufferTwo[14];///character array that stores the values of the lastDistance variable
00510 
00511 
00512 
00513                 float lengthN = sprintf(bufferOne,"Dn = %.2f cm",distance);
00514                 if (lengthN <= 14)
00515                     lcd.printString(bufferTwo,0,4);
00516 
00517                 float lengthL = sprintf(bufferTwo,"Dl = %.2f cm",lastDistance);
00518                 if (lengthL <= 14)
00519                     lcd.printString(bufferTwo,0,5);
00520             }
00521             lcd.refresh();
00522 
00523 
00524             if (rangeFlag) {
00525                 lcd.clear();
00526                 rangeFlag=0;  ///reset  flag
00527                 char buffer[14];
00528 
00529                 //   distance = srf.getDistanceCm();  // same idea with floats
00530                 float length = sprintf(buffer," %.2f cm",distance);
00531                 if (length <= 14) {
00532                     lcd.printString(buffer,11,2);
00533                 }
00534 
00535                 buzzerFrequency=fsm[currentState].bOutput;  /// retrieves the buzzer frequency associated to the currentState from the FSM definition
00536                 buzzer.beep(buzzerFrequency,1);
00537                 float lengthB = sprintf(buffer," %.2f hz",buzzerFrequency);
00538                 if (lengthB <= 14) {
00539                     lcd.printString(buffer,11,4);
00540                 }
00541                 lcd.refresh();
00542             }
00543 
00544 
00545             if (beamFlag) {
00546                 lcd.clear();
00547                 beamFlag =0;
00548 
00549                 z++;
00550                 if (z>=5) z=0;
00551                 angleDegrees = angleArray [z];
00552                 angleRadians=convertDegrees(angleDegrees);
00553                 a=angleRadians;
00554 
00555                 drawSemiCircle();
00556                 d =srf.getDistanceCm();
00557 //simply assigns the function results under conditions (a,d) to the distance variable values. calls the pointers defined previously
00558                 lateralDistance = 2*lat(a,d);
00559                 h = hyp(a,d);
00560 
00561                 drawBeamLines ();
00562 
00563             }
00564             lcd.refresh();
00565 
00566 
00567             if (wallFlag) {
00568                 lcd.clear();
00569                 wallFlag=0; ///reset flag
00570                 drawMaze();  ///draws the screen backgroung structure
00571                 move();    ///controls the movement and associated signals for the moving pixel point
00572             }
00573 
00574         }
00575     }
00576 }
00577 
00578