1
Dependencies: sMotor LIS3MDL X_NUCLEO_53L0A1
main.cpp
- Committer:
- simens
- Date:
- 2019-05-23
- Revision:
- 0:ed3e71232bc7
File content as of revision 0:ed3e71232bc7:
#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; } }