main.cpp

00001 #include "mbed.h"
00002 #include "XNucleo53L0A1.h"
00003 #include "L3G4200D_my.h"
00004 #include "sMotor.h" 
00005 #include "lis3mdl_class.h"
00006 
00007 /****************************************************************
00008  *                        Definitions                           *                                              
00009 *****************************************************************/
00010 
00011 /*****************************************************************
00012  *                         Prototypes                            *       
00013  *****************************************************************/
00014 void button1_enabled_cb(void);
00015 void button1_onpressed_cb(void);
00016 void Ini();
00017 void proximityR_isr();
00018 void proximityF_isr();
00019 void polling_sensors_isr();
00020 void sensors_task();
00021 void execute_pc(int events);
00022 void DurationMesure(uint32_t *dt);
00023 
00024 
00025 
00026 /*****************************************************************
00027 *                     Interface                                  *               
00028 ******************************************************************/
00029 DigitalOut led1(LED1);
00030 InterruptIn button1(USER_BUTTON);
00031 RawSerial pc(USBTX, USBRX);
00032 DevI2C *device_i2c;
00033 static XNucleo53L0A1 *board=NULL;
00034 sMotor motor(A5, A4, A3, A1);
00035 InterruptIn proximity(A0);
00036 DevI2C devI2c(D14,D15);
00037 LIS3MDL magnetometer(&devI2c,LIS3MDL_M_MEMS_ADDRESS_LOW);
00038 
00039 
00040 
00041 /*****************************************************************
00042 *                         Threads                                *
00043 ******************************************************************/
00044 Thread sensor_daemon;
00045 
00046 
00047 /*****************************************************************  
00048 *                     Time                                       *
00049 ******************************************************************/
00050 Timeout button1_timeout; // Used for debouncing
00051 Ticker polling_sensors;
00052 Timer DurationTimer; 
00053 
00054 
00055 /*****************************************************************
00056 *                     Global variables                           *              
00057 ******************************************************************/
00058 volatile bool button1_pressed = false; // Used in the main loop
00059 volatile bool button1_enabled = true; // Used for debouncing
00060 uint8_t mode=0;
00061 uint8_t prox=0;
00062 uint8_t SensorsEn=1;
00063 uint32_t distance_c;
00064 uint32_t distance_l;
00065 uint32_t distance_r;
00066 int16_t G[3];
00067 char rxpc_buffer[128];
00068 uint32_t TaskDurationL; 
00069 uint32_t TaskDurationR;
00070 uint32_t TaskDurationC;
00071 uint32_t TaskDurationG;
00072 uint8_t direction;
00073 uint8_t point = 0;
00074 
00075 
00076 /***************************************************************** 
00077 *                      Main                                      *
00078 ******************************************************************/
00079 int main()
00080 {
00081     Ini();
00082     while (true) {
00083         if(button1_pressed){
00084             mode++;
00085             if(mode>4)
00086                 mode=0;
00087             button1_pressed=false;  
00088         }
00089         if(point){
00090            motor.step(point, direction, 5000); 
00091         }else {
00092            wait(0.05);    
00093         } 
00094     }
00095 }
00096 
00097 /***********************************************************
00098 *            Functions                                     *
00099 ***********************************************************/
00100 void Ini()
00101 { 
00102 //    thread.start(print_thread);
00103     //button1.mode(PullUp); // Activate pull-up
00104     // Attach ISR to handle button press event  
00105     button1.fall(callback(button1_onpressed_cb)); 
00106     pc.baud(115200);
00107     
00108     device_i2c = new DevI2C(D14, D15);
00109     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
00110     int status = board->init_board();
00111     if (status) 
00112         pc.printf("Failed to init XNucleo53L0A1 board!\r\n");
00113     char text[5];
00114     sprintf(text,"mbed");  
00115     board->display->display_string(text);  
00116     GyroL3G4200D_Ini(device_i2c);  
00117     proximity.mode(PullUp);  
00118     proximity.rise(&proximityR_isr); 
00119     proximity.fall(&proximityF_isr);
00120     mode=5;
00121     wait(2.0);
00122     sensor_daemon.start(sensors_task); 
00123     SensorsEn=1;
00124     polling_sensors.attach(&polling_sensors_isr, 0.4);
00125     pc.read((uint8_t *)rxpc_buffer,
00126     128, &execute_pc, SERIAL_EVENT_RX_ALL,10);
00127     
00128 
00129 
00130 
00131 
00132  }
00133 
00134 //-----------------------
00135 void DurationMesure(uint32_t *dt)
00136 {
00137   DurationTimer.stop(); 
00138  *dt=DurationTimer.read_us(); 
00139    DurationTimer.reset();       
00140 }  
00141 
00142 
00143 
00144 
00145 
00146 //-------------------------------------------
00147 void sensors_task()
00148 {
00149  int status;
00150  char text[5]; 
00151  
00152     while (true) {
00153         ThisThread::flags_wait_any(0x1,true);
00154         SensorsEn=0;
00155         //DurationTimer.start();                 
00156         status = board->sensor_left->get_distance(&distance_l); 
00157         if (status != VL53L0X_ERROR_NONE)
00158             distance_l=8888;  
00159         //DurationMesure(&TaskDurationL);
00160         //DurationTimer.start();   
00161         status = board->sensor_right->get_distance(&distance_r);
00162         if (status != VL53L0X_ERROR_NONE)
00163              distance_r=8888;      
00164         //DurationMesure(&TaskDurationR);
00165         //DurationTimer.start();   
00166         status = board->sensor_centre->get_distance(&distance_c); 
00167         if (status != VL53L0X_ERROR_NONE)
00168              distance_c=8888; 
00169        //DurationMesure(&TaskDurationC);
00170        
00171         switch(mode){
00172             case 0:
00173                 point = 0;
00174                 sprintf(text,"c%ld",distance_c);  
00175                 break;
00176             case 1:
00177                 point = 0;
00178                 sprintf(text,"l%ld", distance_l);                        
00179                 break;  
00180             case 2:      
00181                 point = 0;
00182                 sprintf(text,"r%ld", distance_r);
00183                 break;    
00184             case 3:      
00185                 int a = abs((int)distance_r-(int)distance_l);
00186                 sprintf(text,"d%ld",a);
00187                 
00188                 if(a<10){
00189                     point = 0;
00190                 }else{
00191                     if ((int)distance_r>(int)distance_l)
00192                     {
00193                         point = 1;
00194                         direction = 1;
00195                     }
00196                     else 
00197                     {
00198                         point = 1;
00199                         direction = 0;
00200                     }
00201                 }
00202                                        
00203                 break;   
00204             case 4:      
00205             
00206                 sprintf(text,"%d",G[0]);
00207                 direction = 0;
00208                 point = 1;    
00209                 if(prox)
00210                     point = 0;
00211                 break;             
00212             default: 
00213                 sprintf(text,"End");                    
00214                 break;                      
00215         } 
00216         board->display->display_string(text);                                 
00217         SensorsEn=1;
00218    }         
00219 }
00220 //------------------------------
00221 void execute_pc(int events)
00222 {
00223  char *endptr;
00224   
00225  if(SERIAL_EVENT_RX_CHARACTER_MATCH & events)   
00226 switch(rxpc_buffer[0]) {
00227       case 'T':
00228       case 't':
00229           pc.printf("DL=%ld,DR=%ld,DC=%ld,DG=%ld",
00230           TaskDurationL,TaskDurationR,TaskDurationC,TaskDurationG);         
00231           break;   
00232       case 'S':
00233       case 's':
00234             pc.printf("DC=%d, DL=%d, DR=%d, PROX=%d, GYRO=%d,%d,%d",
00235          distance_c,distance_l,distance_r,proximity?1:0,G[0],G[1],G[2]);
00236             break;    
00237       case 'M':
00238       case 'm':        
00239           mode=strtol(&rxpc_buffer[1],&endptr,10);
00240           break;                                     
00241    } 
00242 else
00243  pc.printf("evtnt=%d", events); 
00244 pc.read((uint8_t *)rxpc_buffer, 128,
00245    &execute_pc,SERIAL_EVENT_RX_ALL,10);//(unsigned char)'\n');  
00246 }  
00247 
00248 
00249 /***********************************************************
00250 *         Interrupt Service Routines                       *
00251 ***********************************************************/
00252 //----------------------------ISR handling button pressed event
00253 void button1_onpressed_cb(void)
00254 {
00255     if (button1_enabled) {// Disabled while the button is bouncing
00256         button1_enabled = false;
00257         button1_pressed = true; // To be read by the main loop
00258          // Debounce time 50 ms
00259 button1_timeout.attach(callback(button1_enabled_cb), 0.03); 
00260     }
00261 }
00262 
00263 //---------------------------Enables button when bouncing is over
00264 void button1_enabled_cb(void)
00265 {
00266     button1_enabled = true;
00267 }
00268 //--------------------------------
00269 void proximityR_isr()
00270 {
00271   prox=1;      
00272 } 
00273 //--------------
00274 void proximityF_isr()
00275 { 
00276   prox=0;      
00277 }    
00278 //------------
00279  void polling_sensors_isr()
00280 {
00281  if (SensorsEn){
00282     sensor_daemon.flags_set(0x1);
00283     led1 = !led1; 
00284  }    
00285 }