Revision 0:ed3e71232bc7, committed 2019-05-23

Comitter:

simens

Date:

Thu May 23 05:55:34 2019 +0000

Commit message:

123

Changed in this revision

diff -r 000000000000 -r ed3e71232bc7 .gitignore
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.gitignore	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,4 @@
+.build
+.mbed
+projectfiles
+*.py*

diff -r 000000000000 -r ed3e71232bc7 L3G4200D_my.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D_my.cpp	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,26 @@
+
+#include "L3G4200D_my.h"
+
+void GyroL3G4200D_Ini(DevI2C *gyro)
+{
+    char data_write[2];
+
+    wait(0.02);
+    data_write[0]=CTRL_REG1;  // DR1 DR0 BW1 BW0 PD Zen Yen Xen
+    data_write[1]=0x1f;       //  0   0   0   1  1   1   1   1    dr=100Hz, BW=25
+    gyro->write(I2C_ADDR_GYRO,data_write, 2,0); // 1-no stop
+}
+
+//-----------------------------------------------
+void GyroL3G4200D_GetAxis(DevI2C *gyro,int16_t* g)
+{
+    char data_write[2];
+    char  buffer[6];
+    
+    data_write[0]=OUT_X_L|0x80;
+    gyro->write(I2C_ADDR_GYRO,data_write, 1,1); // 1-no stop
+    gyro->read(I2C_ADDR_GYRO,buffer, 6,0);
+    g[0]=*((int16_t*)&buffer[0]);
+    g[1]=*((int16_t*)&buffer[2]);
+    g[2]=*((int16_t*)&buffer[4]); 
+}

diff -r 000000000000 -r ed3e71232bc7 L3G4200D_my.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D_my.h	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,28 @@
+#ifndef __L3G4200D_MY_H
+#define __L3G4200D_MY_H
+
+#include "mbed.h"
+#include "DevI2C.h"
+
+#define I2C_ADDR_READ 0xd1
+#define I2C_ADDR_WRITE 0xd0
+#define I2C_ADDR_GYRO 0xd0
+
+#define CTRL_REG1 0x20
+#define CTRL_REG2 0x21
+#define CTRL_REG3 0x22
+#define CTRL_REG4 0x23
+#define CTRL_REG5 0x24
+#define STATUS_REG 0x27
+#define OUT_X_L   0x28
+#define OUT_X_H   0x29
+#define OUT_Y_L   0x2a
+#define OUT_Y_H   0x2b
+#define OUT_Z_L   0x2c
+#define OUT_Z_H   0x2d
+
+void GyroL3G4200D_Ini(DevI2C *gyro);
+void GyroL3G4200D_GetAxis(DevI2C *gyro,int16_t* g);
+#endif
+
+

diff -r 000000000000 -r ed3e71232bc7 LIS3MDL.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LIS3MDL.lib	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/ST/code/LIS3MDL/#308889c4d074

diff -r 000000000000 -r ed3e71232bc7 X_NUCLEO_53L0A1.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_53L0A1.lib	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/ST/code/X_NUCLEO_53L0A1/#99c367e8a402

diff -r 000000000000 -r ed3e71232bc7 lis3mdl_class.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lis3mdl_class.cpp	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,12 @@
+#include <lis3mdl_class.h> 
+
+MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_SetAxeOffset(uint8_t axe,int16_t offset)
+{
+  uint8_t tmp[2];
+  tmp[0]=offset;
+  tmp[1]=offset>>8;
+  if(LIS3MDL_IO_Write(tmp, 5+2*axe, 2)!= MAGNETO_OK)
+    return MAGNETO_ERROR;
+  else  
+    return MAGNETO_OK;
+}

diff -r 000000000000 -r ed3e71232bc7 lis3mdl_class.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lis3mdl_class.h	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,23 @@
+#ifndef lis3mdl_class_h
+#define lis3mdl_class_h
+
+class lis3mdl{
+public: 
+    virtual int set_m_axes_offset(uint8_t axe, int16_t offset) {//my
+      return LIS3MDL_M_SetAxeOffset(axe, offset);
+   }
+    virtual int set_BDU() { //my
+      uint8_t tmp;
+      if(LIS3MDL_IO_Read(&tmp,LIS3MDL_M_CTRL_REG5_M,1)!=    
+              MAGNETO_OK)
+           return MAGNETO_ERROR;
+       tmp=0x40;
+      return LIS3MDL_IO_Write(&tmp, LIS3MDL_M_CTRL_REG5_M, 1);
+    }    
+
+protected: 
+    MAGNETO_StatusTypeDef LIS3MDL_M_SetAxeOffset(uint8_t axe,int16_t offset);//my
+    
+};
+
+#endif
\ No newline at end of file

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; 
+ }    
+}

diff -r 000000000000 -r ed3e71232bc7 mbed-os.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#ecb3c8c837162c73537bd0f3592c6e2a42994045

diff -r 000000000000 -r ed3e71232bc7 sMotor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sMotor.lib	Thu May 23 05:55:34 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/XtaticO/code/sMotor/#4b3b9e047ce3