s g
1
Dependencies: sMotor LIS3MDL X_NUCLEO_53L0A1
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
--- /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*
--- /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]);
+}
--- /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 + +
--- /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
--- /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
--- /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;
+}
--- /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
--- /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;
+ }
+}
--- /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
--- /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