function.cpp

00001 #include <stdint.h>
00002 #include <stdio.h>
00003 #include "mbed.h"
00004 #include "XNucleo53L1A1.h"
00005 #include "VL53L1X_I2C.h"
00006 
00007 extern uint16_t distance_centre; //the distance data of the central sensor
00008 extern uint16_t distance_left; //the distance data of the left sensor
00009 extern uint16_t distance_right; //the distance data of the right sensor
00010 extern uint8_t state;
00011 extern uint8_t command;
00012 
00013 static XNucleo53L1A1 *board; //initialize the hardware
00014 DigitalOut myled(LED1); //initialize the led and named it as "myled"
00015 int wait_fun(); //initialize the wait_function
00016 
00017 int wait_fun()
00018 {
00019     if(state == 0) // First stage
00020     {
00021         // instructive led on, device is ready
00022         myled = 1;
00023         // ‘Waiting area’
00024     }
00025     else if( distance_centre > 50 && distance_centre < 80 )
00026     {
00027         command = 3;
00028         state = 11; //forward to the second state
00029                 
00030         myled = 0; //led is off, program begins
00031     }
00032     else if( distance_centre > 400 ) // ‘Stop’
00033     {
00034         command = 9;
00035         state = 0; //Back to the first stage
00036     }
00037     
00038     
00039     if(state == 11) // Second stage
00040     {
00041         // ‘Waiting area’
00042     }
00043         else if(distance_left > 1 && distance_left < 200&& distance_centre >50 && distance_centre < 80 && distance_right > 1 &&distance_right < 200)
00044         {
00045             command = 3;
00046             state = 11; // Back to second stage
00047         }
00048                     
00049         // ‘Turning right’
00050                 
00051         else if(distance_right < 80 && distance_right > 50&&distance_centre < 80 && distance_centre > 50&&distance_left>1000)
00052         {
00053             command=7;
00054             state = 11;
00055         }
00056         //’Turing left’
00057                 
00058         else if(distance_left < 80 && distance_left > 50 &&distance_centre< 80 && distance_centre > 50 &&distance_right>50&&distance_right >1000)
00059         {
00060             command=8;
00061             state = 11;
00062         }
00063         // ‘Going forward’
00064         else if(distance_right > 80 && distance_right < 400 &&distance_centre > 80 && distance_centre < 400 &&distance_left > 80&& distance_left <400)
00065         {
00066             command = 1;
00067             state = 11;
00068         }
00069         // ‘Going left forward’
00070         else if (distance_left < 400 && distance_left > 80 &&distance_centre< 400 && distance_centre > 80 &&distance_right>1000)
00071         {
00072             command = 5;
00073             state =11 ;
00074         }
00075            
00076         // ‘Going right forward’
00077         else if (distance_right < 400 && distance_right > 8&&distance_centre < 400 && distance_centre > 80&&distance_left>1000)
00078         {
00079             command =6;
00080             state =11 ;
00081         }
00082         // ‘Going back’
00083         else if(distance_left < 50 && distance_left >1 && distance_centre <50 && distance_centre >1 && distance_right < 50 &&distance_right >1 )
00084         {
00085             command=2;
00086             state = 11;
00087         }
00088         // ‘Going left backward’
00089         else if (distance_left < 50 && distance_left > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_right>1000)
00090         {
00091             command =55;
00092             state =0 ;
00093         }
00094         // ‘Going right backward’
00095         else if (distance_right < 50 && distance_right > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_left>1000)
00096         {
00097             command =66;
00098             state =0 ;
00099         }
00100         // ‘Stop’
00101         else if( distance_centre > 400 )
00102         {
00103             command = 9;
00104             state = 0;
00105         }
00106     
00107     return 0;
00108 }