s g / Mbed OS EXAM1

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?

UserRevisionLine numberNew 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 }