Diff: main.cpp

Revision:

0:ed3e71232bc7

diff -r 000000000000 -r ed3e71232bc7 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,285 @@
+#include "mbed.h"
+#include "XNucleo53L0A1.h"
+#include "L3G4200D_my.h"
+#include "sMotor.h" 
+#include "lis3mdl_class.h"
+
+/****************************************************************
+ *                        Definitions                           *                                              
+*****************************************************************/
+
+/*****************************************************************
+ *                         Prototypes                            *       
+ *****************************************************************/
+void button1_enabled_cb(void);
+void button1_onpressed_cb(void);
+void Ini();
+void proximityR_isr();
+void proximityF_isr();
+void polling_sensors_isr();
+void sensors_task();
+void execute_pc(int events);
+void DurationMesure(uint32_t *dt);
+
+
+
+/*****************************************************************
+*                     Interface                                  *               
+******************************************************************/
+DigitalOut led1(LED1);
+InterruptIn button1(USER_BUTTON);
+RawSerial pc(USBTX, USBRX);
+DevI2C *device_i2c;
+static XNucleo53L0A1 *board=NULL;
+sMotor motor(A5, A4, A3, A1);
+InterruptIn proximity(A0);
+DevI2C devI2c(D14,D15);
+LIS3MDL magnetometer(&devI2c,LIS3MDL_M_MEMS_ADDRESS_LOW);
+
+
+
+/*****************************************************************
+*                         Threads                                *
+******************************************************************/
+Thread sensor_daemon;
+
+
+/*****************************************************************  
+*                     Time                                       *
+******************************************************************/
+Timeout button1_timeout; // Used for debouncing
+Ticker polling_sensors;
+Timer DurationTimer; 
+
+
+/*****************************************************************
+*                     Global variables                           *              
+******************************************************************/
+volatile bool button1_pressed = false; // Used in the main loop
+volatile bool button1_enabled = true; // Used for debouncing
+uint8_t mode=0;
+uint8_t prox=0;
+uint8_t SensorsEn=1;
+uint32_t distance_c;
+uint32_t distance_l;
+uint32_t distance_r;
+int16_t G[3];
+char rxpc_buffer[128];
+uint32_t TaskDurationL; 
+uint32_t TaskDurationR;
+uint32_t TaskDurationC;
+uint32_t TaskDurationG;
+uint8_t direction;
+uint8_t point = 0;
+
+
+/***************************************************************** 
+*                      Main                                      *
+******************************************************************/
+int main()
+{
+    Ini();
+    while (true) {
+        if(button1_pressed){
+            mode++;
+            if(mode>4)
+                mode=0;
+            button1_pressed=false;  
+        }
+        if(point){
+           motor.step(point, direction, 5000); 
+        }else {
+           wait(0.05);    
+        } 
+    }
+}
+
+/***********************************************************
+*            Functions                                     *
+***********************************************************/
+void Ini()
+{ 
+//    thread.start(print_thread);
+    //button1.mode(PullUp); // Activate pull-up
+    // Attach ISR to handle button press event  
+    button1.fall(callback(button1_onpressed_cb)); 
+    pc.baud(115200);
+    
+    device_i2c = new DevI2C(D14, D15);
+    board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
+    int status = board->init_board();
+    if (status) 
+        pc.printf("Failed to init XNucleo53L0A1 board!\r\n");
+    char text[5];
+    sprintf(text,"mbed");  
+    board->display->display_string(text);  
+    GyroL3G4200D_Ini(device_i2c);  
+    proximity.mode(PullUp);  
+    proximity.rise(&proximityR_isr); 
+    proximity.fall(&proximityF_isr);
+    mode=5;
+    wait(2.0);
+    sensor_daemon.start(sensors_task); 
+    SensorsEn=1;
+    polling_sensors.attach(&polling_sensors_isr, 0.4);
+    pc.read((uint8_t *)rxpc_buffer,
+    128, &execute_pc, SERIAL_EVENT_RX_ALL,10);
+    
+
+
+
+
+ }
+
+//-----------------------
+void DurationMesure(uint32_t *dt)
+{
+  DurationTimer.stop(); 
+ *dt=DurationTimer.read_us(); 
+   DurationTimer.reset();       
+}  
+
+
+
+
+
+//-------------------------------------------
+void sensors_task()
+{
+ int status;
+ char text[5]; 
+ 
+    while (true) {
+        ThisThread::flags_wait_any(0x1,true);
+        SensorsEn=0;
+        //DurationTimer.start();                 
+        status = board->sensor_left->get_distance(&distance_l); 
+        if (status != VL53L0X_ERROR_NONE)
+            distance_l=8888;  
+        //DurationMesure(&TaskDurationL);
+        //DurationTimer.start();   
+        status = board->sensor_right->get_distance(&distance_r);
+        if (status != VL53L0X_ERROR_NONE)
+             distance_r=8888;      
+        //DurationMesure(&TaskDurationR);
+        //DurationTimer.start();   
+        status = board->sensor_centre->get_distance(&distance_c); 
+        if (status != VL53L0X_ERROR_NONE)
+             distance_c=8888; 
+       //DurationMesure(&TaskDurationC);
+       
+        switch(mode){
+            case 0:
+                point = 0;
+                sprintf(text,"c%ld",distance_c);  
+                break;
+            case 1:
+                point = 0;
+                sprintf(text,"l%ld", distance_l);                        
+                break;  
+            case 2:      
+                point = 0;
+                sprintf(text,"r%ld", distance_r);
+                break;    
+            case 3:      
+                int a = abs((int)distance_r-(int)distance_l);
+                sprintf(text,"d%ld",a);
+                
+                if(a<10){
+                    point = 0;
+                }else{
+                    if ((int)distance_r>(int)distance_l)
+                    {
+                        point = 1;
+                        direction = 1;
+                    }
+                    else 
+                    {
+                        point = 1;
+                        direction = 0;
+                    }
+                }
+                                       
+                break;   
+            case 4:      
+            
+                sprintf(text,"%d",G[0]);
+                direction = 0;
+                point = 1;    
+                if(prox)
+                    point = 0;
+                break;             
+            default: 
+                sprintf(text,"End");                    
+                break;                      
+        } 
+        board->display->display_string(text);                                 
+        SensorsEn=1;
+   }         
+}
+//------------------------------
+void execute_pc(int events)
+{
+ char *endptr;
+  
+ if(SERIAL_EVENT_RX_CHARACTER_MATCH & events)   
+switch(rxpc_buffer[0]) {
+      case 'T':
+      case 't':
+          pc.printf("DL=%ld,DR=%ld,DC=%ld,DG=%ld",
+          TaskDurationL,TaskDurationR,TaskDurationC,TaskDurationG);         
+          break;   
+      case 'S':
+      case 's':
+            pc.printf("DC=%d, DL=%d, DR=%d, PROX=%d, GYRO=%d,%d,%d",
+         distance_c,distance_l,distance_r,proximity?1:0,G[0],G[1],G[2]);
+            break;    
+      case 'M':
+      case 'm':        
+          mode=strtol(&rxpc_buffer[1],&endptr,10);
+          break;                                     
+   } 
+else
+ pc.printf("evtnt=%d", events); 
+pc.read((uint8_t *)rxpc_buffer, 128,
+   &execute_pc,SERIAL_EVENT_RX_ALL,10);//(unsigned char)'\n');  
+}  
+
+
+/***********************************************************
+*         Interrupt Service Routines                       *
+***********************************************************/
+//----------------------------ISR handling button pressed event
+void button1_onpressed_cb(void)
+{
+    if (button1_enabled) {// Disabled while the button is bouncing
+        button1_enabled = false;
+        button1_pressed = true; // To be read by the main loop
+         // Debounce time 50 ms
+button1_timeout.attach(callback(button1_enabled_cb), 0.03); 
+    }
+}
+
+//---------------------------Enables button when bouncing is over
+void button1_enabled_cb(void)
+{
+    button1_enabled = true;
+}
+//--------------------------------
+void proximityR_isr()
+{
+  prox=1;      
+} 
+//--------------
+void proximityF_isr()
+{ 
+  prox=0;      
+}    
+//------------
+ void polling_sensors_isr()
+{
+ if (SensorsEn){
+    sensor_daemon.flags_set(0x1);
+    led1 = !led1; 
+ }    
+}