![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
1
Dependencies: sMotor LIS3MDL X_NUCLEO_53L0A1
Diff: main.cpp
- Revision:
- 0:ed3e71232bc7
--- /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; + } +}