1
Dependencies: sMotor LIS3MDL X_NUCLEO_53L0A1
main.cpp@0:ed3e71232bc7, 2019-05-23 (annotated)
- Committer:
- simens
- Date:
- Thu May 23 05:55:34 2019 +0000
- Revision:
- 0:ed3e71232bc7
123
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
simens | 0:ed3e71232bc7 | 1 | #include "mbed.h" |
simens | 0:ed3e71232bc7 | 2 | #include "XNucleo53L0A1.h" |
simens | 0:ed3e71232bc7 | 3 | #include "L3G4200D_my.h" |
simens | 0:ed3e71232bc7 | 4 | #include "sMotor.h" |
simens | 0:ed3e71232bc7 | 5 | #include "lis3mdl_class.h" |
simens | 0:ed3e71232bc7 | 6 | |
simens | 0:ed3e71232bc7 | 7 | /**************************************************************** |
simens | 0:ed3e71232bc7 | 8 | * Definitions * |
simens | 0:ed3e71232bc7 | 9 | *****************************************************************/ |
simens | 0:ed3e71232bc7 | 10 | |
simens | 0:ed3e71232bc7 | 11 | /***************************************************************** |
simens | 0:ed3e71232bc7 | 12 | * Prototypes * |
simens | 0:ed3e71232bc7 | 13 | *****************************************************************/ |
simens | 0:ed3e71232bc7 | 14 | void button1_enabled_cb(void); |
simens | 0:ed3e71232bc7 | 15 | void button1_onpressed_cb(void); |
simens | 0:ed3e71232bc7 | 16 | void Ini(); |
simens | 0:ed3e71232bc7 | 17 | void proximityR_isr(); |
simens | 0:ed3e71232bc7 | 18 | void proximityF_isr(); |
simens | 0:ed3e71232bc7 | 19 | void polling_sensors_isr(); |
simens | 0:ed3e71232bc7 | 20 | void sensors_task(); |
simens | 0:ed3e71232bc7 | 21 | void execute_pc(int events); |
simens | 0:ed3e71232bc7 | 22 | void DurationMesure(uint32_t *dt); |
simens | 0:ed3e71232bc7 | 23 | |
simens | 0:ed3e71232bc7 | 24 | |
simens | 0:ed3e71232bc7 | 25 | |
simens | 0:ed3e71232bc7 | 26 | /***************************************************************** |
simens | 0:ed3e71232bc7 | 27 | * Interface * |
simens | 0:ed3e71232bc7 | 28 | ******************************************************************/ |
simens | 0:ed3e71232bc7 | 29 | DigitalOut led1(LED1); |
simens | 0:ed3e71232bc7 | 30 | InterruptIn button1(USER_BUTTON); |
simens | 0:ed3e71232bc7 | 31 | RawSerial pc(USBTX, USBRX); |
simens | 0:ed3e71232bc7 | 32 | DevI2C *device_i2c; |
simens | 0:ed3e71232bc7 | 33 | static XNucleo53L0A1 *board=NULL; |
simens | 0:ed3e71232bc7 | 34 | sMotor motor(A5, A4, A3, A1); |
simens | 0:ed3e71232bc7 | 35 | InterruptIn proximity(A0); |
simens | 0:ed3e71232bc7 | 36 | DevI2C devI2c(D14,D15); |
simens | 0:ed3e71232bc7 | 37 | LIS3MDL magnetometer(&devI2c,LIS3MDL_M_MEMS_ADDRESS_LOW); |
simens | 0:ed3e71232bc7 | 38 | |
simens | 0:ed3e71232bc7 | 39 | |
simens | 0:ed3e71232bc7 | 40 | |
simens | 0:ed3e71232bc7 | 41 | /***************************************************************** |
simens | 0:ed3e71232bc7 | 42 | * Threads * |
simens | 0:ed3e71232bc7 | 43 | ******************************************************************/ |
simens | 0:ed3e71232bc7 | 44 | Thread sensor_daemon; |
simens | 0:ed3e71232bc7 | 45 | |
simens | 0:ed3e71232bc7 | 46 | |
simens | 0:ed3e71232bc7 | 47 | /***************************************************************** |
simens | 0:ed3e71232bc7 | 48 | * Time * |
simens | 0:ed3e71232bc7 | 49 | ******************************************************************/ |
simens | 0:ed3e71232bc7 | 50 | Timeout button1_timeout; // Used for debouncing |
simens | 0:ed3e71232bc7 | 51 | Ticker polling_sensors; |
simens | 0:ed3e71232bc7 | 52 | Timer DurationTimer; |
simens | 0:ed3e71232bc7 | 53 | |
simens | 0:ed3e71232bc7 | 54 | |
simens | 0:ed3e71232bc7 | 55 | /***************************************************************** |
simens | 0:ed3e71232bc7 | 56 | * Global variables * |
simens | 0:ed3e71232bc7 | 57 | ******************************************************************/ |
simens | 0:ed3e71232bc7 | 58 | volatile bool button1_pressed = false; // Used in the main loop |
simens | 0:ed3e71232bc7 | 59 | volatile bool button1_enabled = true; // Used for debouncing |
simens | 0:ed3e71232bc7 | 60 | uint8_t mode=0; |
simens | 0:ed3e71232bc7 | 61 | uint8_t prox=0; |
simens | 0:ed3e71232bc7 | 62 | uint8_t SensorsEn=1; |
simens | 0:ed3e71232bc7 | 63 | uint32_t distance_c; |
simens | 0:ed3e71232bc7 | 64 | uint32_t distance_l; |
simens | 0:ed3e71232bc7 | 65 | uint32_t distance_r; |
simens | 0:ed3e71232bc7 | 66 | int16_t G[3]; |
simens | 0:ed3e71232bc7 | 67 | char rxpc_buffer[128]; |
simens | 0:ed3e71232bc7 | 68 | uint32_t TaskDurationL; |
simens | 0:ed3e71232bc7 | 69 | uint32_t TaskDurationR; |
simens | 0:ed3e71232bc7 | 70 | uint32_t TaskDurationC; |
simens | 0:ed3e71232bc7 | 71 | uint32_t TaskDurationG; |
simens | 0:ed3e71232bc7 | 72 | uint8_t direction; |
simens | 0:ed3e71232bc7 | 73 | uint8_t point = 0; |
simens | 0:ed3e71232bc7 | 74 | |
simens | 0:ed3e71232bc7 | 75 | |
simens | 0:ed3e71232bc7 | 76 | /***************************************************************** |
simens | 0:ed3e71232bc7 | 77 | * Main * |
simens | 0:ed3e71232bc7 | 78 | ******************************************************************/ |
simens | 0:ed3e71232bc7 | 79 | int main() |
simens | 0:ed3e71232bc7 | 80 | { |
simens | 0:ed3e71232bc7 | 81 | Ini(); |
simens | 0:ed3e71232bc7 | 82 | while (true) { |
simens | 0:ed3e71232bc7 | 83 | if(button1_pressed){ |
simens | 0:ed3e71232bc7 | 84 | mode++; |
simens | 0:ed3e71232bc7 | 85 | if(mode>4) |
simens | 0:ed3e71232bc7 | 86 | mode=0; |
simens | 0:ed3e71232bc7 | 87 | button1_pressed=false; |
simens | 0:ed3e71232bc7 | 88 | } |
simens | 0:ed3e71232bc7 | 89 | if(point){ |
simens | 0:ed3e71232bc7 | 90 | motor.step(point, direction, 5000); |
simens | 0:ed3e71232bc7 | 91 | }else { |
simens | 0:ed3e71232bc7 | 92 | wait(0.05); |
simens | 0:ed3e71232bc7 | 93 | } |
simens | 0:ed3e71232bc7 | 94 | } |
simens | 0:ed3e71232bc7 | 95 | } |
simens | 0:ed3e71232bc7 | 96 | |
simens | 0:ed3e71232bc7 | 97 | /*********************************************************** |
simens | 0:ed3e71232bc7 | 98 | * Functions * |
simens | 0:ed3e71232bc7 | 99 | ***********************************************************/ |
simens | 0:ed3e71232bc7 | 100 | void Ini() |
simens | 0:ed3e71232bc7 | 101 | { |
simens | 0:ed3e71232bc7 | 102 | // thread.start(print_thread); |
simens | 0:ed3e71232bc7 | 103 | //button1.mode(PullUp); // Activate pull-up |
simens | 0:ed3e71232bc7 | 104 | // Attach ISR to handle button press event |
simens | 0:ed3e71232bc7 | 105 | button1.fall(callback(button1_onpressed_cb)); |
simens | 0:ed3e71232bc7 | 106 | pc.baud(115200); |
simens | 0:ed3e71232bc7 | 107 | |
simens | 0:ed3e71232bc7 | 108 | device_i2c = new DevI2C(D14, D15); |
simens | 0:ed3e71232bc7 | 109 | board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2); |
simens | 0:ed3e71232bc7 | 110 | int status = board->init_board(); |
simens | 0:ed3e71232bc7 | 111 | if (status) |
simens | 0:ed3e71232bc7 | 112 | pc.printf("Failed to init XNucleo53L0A1 board!\r\n"); |
simens | 0:ed3e71232bc7 | 113 | char text[5]; |
simens | 0:ed3e71232bc7 | 114 | sprintf(text,"mbed"); |
simens | 0:ed3e71232bc7 | 115 | board->display->display_string(text); |
simens | 0:ed3e71232bc7 | 116 | GyroL3G4200D_Ini(device_i2c); |
simens | 0:ed3e71232bc7 | 117 | proximity.mode(PullUp); |
simens | 0:ed3e71232bc7 | 118 | proximity.rise(&proximityR_isr); |
simens | 0:ed3e71232bc7 | 119 | proximity.fall(&proximityF_isr); |
simens | 0:ed3e71232bc7 | 120 | mode=5; |
simens | 0:ed3e71232bc7 | 121 | wait(2.0); |
simens | 0:ed3e71232bc7 | 122 | sensor_daemon.start(sensors_task); |
simens | 0:ed3e71232bc7 | 123 | SensorsEn=1; |
simens | 0:ed3e71232bc7 | 124 | polling_sensors.attach(&polling_sensors_isr, 0.4); |
simens | 0:ed3e71232bc7 | 125 | pc.read((uint8_t *)rxpc_buffer, |
simens | 0:ed3e71232bc7 | 126 | 128, &execute_pc, SERIAL_EVENT_RX_ALL,10); |
simens | 0:ed3e71232bc7 | 127 | |
simens | 0:ed3e71232bc7 | 128 | |
simens | 0:ed3e71232bc7 | 129 | |
simens | 0:ed3e71232bc7 | 130 | |
simens | 0:ed3e71232bc7 | 131 | |
simens | 0:ed3e71232bc7 | 132 | } |
simens | 0:ed3e71232bc7 | 133 | |
simens | 0:ed3e71232bc7 | 134 | //----------------------- |
simens | 0:ed3e71232bc7 | 135 | void DurationMesure(uint32_t *dt) |
simens | 0:ed3e71232bc7 | 136 | { |
simens | 0:ed3e71232bc7 | 137 | DurationTimer.stop(); |
simens | 0:ed3e71232bc7 | 138 | *dt=DurationTimer.read_us(); |
simens | 0:ed3e71232bc7 | 139 | DurationTimer.reset(); |
simens | 0:ed3e71232bc7 | 140 | } |
simens | 0:ed3e71232bc7 | 141 | |
simens | 0:ed3e71232bc7 | 142 | |
simens | 0:ed3e71232bc7 | 143 | |
simens | 0:ed3e71232bc7 | 144 | |
simens | 0:ed3e71232bc7 | 145 | |
simens | 0:ed3e71232bc7 | 146 | //------------------------------------------- |
simens | 0:ed3e71232bc7 | 147 | void sensors_task() |
simens | 0:ed3e71232bc7 | 148 | { |
simens | 0:ed3e71232bc7 | 149 | int status; |
simens | 0:ed3e71232bc7 | 150 | char text[5]; |
simens | 0:ed3e71232bc7 | 151 | |
simens | 0:ed3e71232bc7 | 152 | while (true) { |
simens | 0:ed3e71232bc7 | 153 | ThisThread::flags_wait_any(0x1,true); |
simens | 0:ed3e71232bc7 | 154 | SensorsEn=0; |
simens | 0:ed3e71232bc7 | 155 | //DurationTimer.start(); |
simens | 0:ed3e71232bc7 | 156 | status = board->sensor_left->get_distance(&distance_l); |
simens | 0:ed3e71232bc7 | 157 | if (status != VL53L0X_ERROR_NONE) |
simens | 0:ed3e71232bc7 | 158 | distance_l=8888; |
simens | 0:ed3e71232bc7 | 159 | //DurationMesure(&TaskDurationL); |
simens | 0:ed3e71232bc7 | 160 | //DurationTimer.start(); |
simens | 0:ed3e71232bc7 | 161 | status = board->sensor_right->get_distance(&distance_r); |
simens | 0:ed3e71232bc7 | 162 | if (status != VL53L0X_ERROR_NONE) |
simens | 0:ed3e71232bc7 | 163 | distance_r=8888; |
simens | 0:ed3e71232bc7 | 164 | //DurationMesure(&TaskDurationR); |
simens | 0:ed3e71232bc7 | 165 | //DurationTimer.start(); |
simens | 0:ed3e71232bc7 | 166 | status = board->sensor_centre->get_distance(&distance_c); |
simens | 0:ed3e71232bc7 | 167 | if (status != VL53L0X_ERROR_NONE) |
simens | 0:ed3e71232bc7 | 168 | distance_c=8888; |
simens | 0:ed3e71232bc7 | 169 | //DurationMesure(&TaskDurationC); |
simens | 0:ed3e71232bc7 | 170 | |
simens | 0:ed3e71232bc7 | 171 | switch(mode){ |
simens | 0:ed3e71232bc7 | 172 | case 0: |
simens | 0:ed3e71232bc7 | 173 | point = 0; |
simens | 0:ed3e71232bc7 | 174 | sprintf(text,"c%ld",distance_c); |
simens | 0:ed3e71232bc7 | 175 | break; |
simens | 0:ed3e71232bc7 | 176 | case 1: |
simens | 0:ed3e71232bc7 | 177 | point = 0; |
simens | 0:ed3e71232bc7 | 178 | sprintf(text,"l%ld", distance_l); |
simens | 0:ed3e71232bc7 | 179 | break; |
simens | 0:ed3e71232bc7 | 180 | case 2: |
simens | 0:ed3e71232bc7 | 181 | point = 0; |
simens | 0:ed3e71232bc7 | 182 | sprintf(text,"r%ld", distance_r); |
simens | 0:ed3e71232bc7 | 183 | break; |
simens | 0:ed3e71232bc7 | 184 | case 3: |
simens | 0:ed3e71232bc7 | 185 | int a = abs((int)distance_r-(int)distance_l); |
simens | 0:ed3e71232bc7 | 186 | sprintf(text,"d%ld",a); |
simens | 0:ed3e71232bc7 | 187 | |
simens | 0:ed3e71232bc7 | 188 | if(a<10){ |
simens | 0:ed3e71232bc7 | 189 | point = 0; |
simens | 0:ed3e71232bc7 | 190 | }else{ |
simens | 0:ed3e71232bc7 | 191 | if ((int)distance_r>(int)distance_l) |
simens | 0:ed3e71232bc7 | 192 | { |
simens | 0:ed3e71232bc7 | 193 | point = 1; |
simens | 0:ed3e71232bc7 | 194 | direction = 1; |
simens | 0:ed3e71232bc7 | 195 | } |
simens | 0:ed3e71232bc7 | 196 | else |
simens | 0:ed3e71232bc7 | 197 | { |
simens | 0:ed3e71232bc7 | 198 | point = 1; |
simens | 0:ed3e71232bc7 | 199 | direction = 0; |
simens | 0:ed3e71232bc7 | 200 | } |
simens | 0:ed3e71232bc7 | 201 | } |
simens | 0:ed3e71232bc7 | 202 | |
simens | 0:ed3e71232bc7 | 203 | break; |
simens | 0:ed3e71232bc7 | 204 | case 4: |
simens | 0:ed3e71232bc7 | 205 | |
simens | 0:ed3e71232bc7 | 206 | sprintf(text,"%d",G[0]); |
simens | 0:ed3e71232bc7 | 207 | direction = 0; |
simens | 0:ed3e71232bc7 | 208 | point = 1; |
simens | 0:ed3e71232bc7 | 209 | if(prox) |
simens | 0:ed3e71232bc7 | 210 | point = 0; |
simens | 0:ed3e71232bc7 | 211 | break; |
simens | 0:ed3e71232bc7 | 212 | default: |
simens | 0:ed3e71232bc7 | 213 | sprintf(text,"End"); |
simens | 0:ed3e71232bc7 | 214 | break; |
simens | 0:ed3e71232bc7 | 215 | } |
simens | 0:ed3e71232bc7 | 216 | board->display->display_string(text); |
simens | 0:ed3e71232bc7 | 217 | SensorsEn=1; |
simens | 0:ed3e71232bc7 | 218 | } |
simens | 0:ed3e71232bc7 | 219 | } |
simens | 0:ed3e71232bc7 | 220 | //------------------------------ |
simens | 0:ed3e71232bc7 | 221 | void execute_pc(int events) |
simens | 0:ed3e71232bc7 | 222 | { |
simens | 0:ed3e71232bc7 | 223 | char *endptr; |
simens | 0:ed3e71232bc7 | 224 | |
simens | 0:ed3e71232bc7 | 225 | if(SERIAL_EVENT_RX_CHARACTER_MATCH & events) |
simens | 0:ed3e71232bc7 | 226 | switch(rxpc_buffer[0]) { |
simens | 0:ed3e71232bc7 | 227 | case 'T': |
simens | 0:ed3e71232bc7 | 228 | case 't': |
simens | 0:ed3e71232bc7 | 229 | pc.printf("DL=%ld,DR=%ld,DC=%ld,DG=%ld", |
simens | 0:ed3e71232bc7 | 230 | TaskDurationL,TaskDurationR,TaskDurationC,TaskDurationG); |
simens | 0:ed3e71232bc7 | 231 | break; |
simens | 0:ed3e71232bc7 | 232 | case 'S': |
simens | 0:ed3e71232bc7 | 233 | case 's': |
simens | 0:ed3e71232bc7 | 234 | pc.printf("DC=%d, DL=%d, DR=%d, PROX=%d, GYRO=%d,%d,%d", |
simens | 0:ed3e71232bc7 | 235 | distance_c,distance_l,distance_r,proximity?1:0,G[0],G[1],G[2]); |
simens | 0:ed3e71232bc7 | 236 | break; |
simens | 0:ed3e71232bc7 | 237 | case 'M': |
simens | 0:ed3e71232bc7 | 238 | case 'm': |
simens | 0:ed3e71232bc7 | 239 | mode=strtol(&rxpc_buffer[1],&endptr,10); |
simens | 0:ed3e71232bc7 | 240 | break; |
simens | 0:ed3e71232bc7 | 241 | } |
simens | 0:ed3e71232bc7 | 242 | else |
simens | 0:ed3e71232bc7 | 243 | pc.printf("evtnt=%d", events); |
simens | 0:ed3e71232bc7 | 244 | pc.read((uint8_t *)rxpc_buffer, 128, |
simens | 0:ed3e71232bc7 | 245 | &execute_pc,SERIAL_EVENT_RX_ALL,10);//(unsigned char)'\n'); |
simens | 0:ed3e71232bc7 | 246 | } |
simens | 0:ed3e71232bc7 | 247 | |
simens | 0:ed3e71232bc7 | 248 | |
simens | 0:ed3e71232bc7 | 249 | /*********************************************************** |
simens | 0:ed3e71232bc7 | 250 | * Interrupt Service Routines * |
simens | 0:ed3e71232bc7 | 251 | ***********************************************************/ |
simens | 0:ed3e71232bc7 | 252 | //----------------------------ISR handling button pressed event |
simens | 0:ed3e71232bc7 | 253 | void button1_onpressed_cb(void) |
simens | 0:ed3e71232bc7 | 254 | { |
simens | 0:ed3e71232bc7 | 255 | if (button1_enabled) {// Disabled while the button is bouncing |
simens | 0:ed3e71232bc7 | 256 | button1_enabled = false; |
simens | 0:ed3e71232bc7 | 257 | button1_pressed = true; // To be read by the main loop |
simens | 0:ed3e71232bc7 | 258 | // Debounce time 50 ms |
simens | 0:ed3e71232bc7 | 259 | button1_timeout.attach(callback(button1_enabled_cb), 0.03); |
simens | 0:ed3e71232bc7 | 260 | } |
simens | 0:ed3e71232bc7 | 261 | } |
simens | 0:ed3e71232bc7 | 262 | |
simens | 0:ed3e71232bc7 | 263 | //---------------------------Enables button when bouncing is over |
simens | 0:ed3e71232bc7 | 264 | void button1_enabled_cb(void) |
simens | 0:ed3e71232bc7 | 265 | { |
simens | 0:ed3e71232bc7 | 266 | button1_enabled = true; |
simens | 0:ed3e71232bc7 | 267 | } |
simens | 0:ed3e71232bc7 | 268 | //-------------------------------- |
simens | 0:ed3e71232bc7 | 269 | void proximityR_isr() |
simens | 0:ed3e71232bc7 | 270 | { |
simens | 0:ed3e71232bc7 | 271 | prox=1; |
simens | 0:ed3e71232bc7 | 272 | } |
simens | 0:ed3e71232bc7 | 273 | //-------------- |
simens | 0:ed3e71232bc7 | 274 | void proximityF_isr() |
simens | 0:ed3e71232bc7 | 275 | { |
simens | 0:ed3e71232bc7 | 276 | prox=0; |
simens | 0:ed3e71232bc7 | 277 | } |
simens | 0:ed3e71232bc7 | 278 | //------------ |
simens | 0:ed3e71232bc7 | 279 | void polling_sensors_isr() |
simens | 0:ed3e71232bc7 | 280 | { |
simens | 0:ed3e71232bc7 | 281 | if (SensorsEn){ |
simens | 0:ed3e71232bc7 | 282 | sensor_daemon.flags_set(0x1); |
simens | 0:ed3e71232bc7 | 283 | led1 = !led1; |
simens | 0:ed3e71232bc7 | 284 | } |
simens | 0:ed3e71232bc7 | 285 | } |