SCRIBE / Mbed 2 deprecated SCRIBE_servo

The servo version of SCRIBE

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed

Fork of SCRIBE_stepper by SCRIBE

main.cpp@3:63aef644e6d2, 2016-04-23 (annotated)

Committer:
manz
Date:
Sat Apr 23 02:46:30 2016 +0000
Revision:
3:63aef644e6d2
Parent:
0:83acd45a2405
Child:
5:1da4d4050306
bluetooth working with new app

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manz 0:83acd45a2405 1 // main of SCRIBE stepper
manz 0:83acd45a2405 2
manz 0:83acd45a2405 3 // Import libraries
manz 0:83acd45a2405 4 #include "Arduino.h"
manz 0:83acd45a2405 5 #include "BLEPeripheral.h"
manz 0:83acd45a2405 6 #include "mbed.h"
manz 0:83acd45a2405 7 #include "stepper.h"
manz 0:83acd45a2405 8 #include "rtos.h"
manz 0:83acd45a2405 9 #include "localization.h";
manz 0:83acd45a2405 10
manz 0:83acd45a2405 11 Serial serial(USBTX, USBRX);
manz 0:83acd45a2405 12
manz 0:83acd45a2405 13 SPI spi(p11, p12, p13);
manz 0:83acd45a2405 14 DigitalInOut BLE_RDY(p21);
manz 0:83acd45a2405 15 DigitalInOut BLE_REQ(p22);
manz 0:83acd45a2405 16 DigitalInOut BLE_RESET(p23);
manz 0:83acd45a2405 17
manz 0:83acd45a2405 18 unsigned char txbuf[16] = {0};
manz 0:83acd45a2405 19 unsigned char txlen = 0;
manz 0:83acd45a2405 20
manz 0:83acd45a2405 21 // create peripheral
manz 0:83acd45a2405 22 BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET);
manz 0:83acd45a2405 23
manz 0:83acd45a2405 24 // create service w/ uuid
manz 0:83acd45a2405 25 BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e");
manz 0:83acd45a2405 26
manz 0:83acd45a2405 27 // create characteristics
manz 0:83acd45a2405 28 BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
manz 0:83acd45a2405 29 BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
manz 0:83acd45a2405 30
manz 0:83acd45a2405 31 unsigned int interval = 0;
manz 0:83acd45a2405 32 unsigned char count_on = 0;
manz 0:83acd45a2405 33
manz 0:83acd45a2405 34 // array to save the values of the user
manz 3:63aef644e6d2 35 const int size = 400;
manz 0:83acd45a2405 36 int path_x [size];
manz 0:83acd45a2405 37 int path_y [size];
manz 0:83acd45a2405 38 int counter = 0;
manz 0:83acd45a2405 39 int start = 0;
manz 0:83acd45a2405 40
manz 3:63aef644e6d2 41 const int shape_size = 10;
manz 3:63aef644e6d2 42 int shape_x [shape_size];
manz 3:63aef644e6d2 43 int shape_y [shape_size];
manz 3:63aef644e6d2 44
manz 3:63aef644e6d2 45 int mode = -1;
manz 3:63aef644e6d2 46
manz 0:83acd45a2405 47 //coordinates of the pen and the centre of SCRIBE at beginning
manz 0:83acd45a2405 48 double x_pen = 0;
manz 0:83acd45a2405 49 double y_pen = 0;
manz 0:83acd45a2405 50 double x_cent = 0;
manz 0:83acd45a2405 51 double y_cent = -1;
manz 0:83acd45a2405 52
manz 0:83acd45a2405 53 //speed of SCRIBE
manz 0:83acd45a2405 54 float speed = 0.3;
manz 0:83acd45a2405 55 float t_factor = 0.15;
manz 0:83acd45a2405 56
manz 0:83acd45a2405 57
manz 0:83acd45a2405 58 Semaphore one_slot(1);
manz 0:83acd45a2405 59
manz 3:63aef644e6d2 60 localization L;
manz 0:83acd45a2405 61
manz 0:83acd45a2405 62 void bluetooth_thread(void const *args){
manz 0:83acd45a2405 63 serial.printf("Serial begin!\r\n");
manz 0:83acd45a2405 64 int x,y,x_dir,y_dir;
manz 0:83acd45a2405 65
manz 0:83acd45a2405 66 /*----- BLE Utility ---------------------------------------------*/
manz 0:83acd45a2405 67 // set advertised local name and service UUID
manz 0:83acd45a2405 68 blePeripheral.setLocalName("BLE Shield");
manz 0:83acd45a2405 69
manz 0:83acd45a2405 70 blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
manz 0:83acd45a2405 71
manz 0:83acd45a2405 72 // add service and characteristic
manz 0:83acd45a2405 73 blePeripheral.addAttribute(uartService);
manz 0:83acd45a2405 74 blePeripheral.addAttribute(rxCharacteristic);
manz 0:83acd45a2405 75 blePeripheral.addAttribute(txCharacteristic);
manz 0:83acd45a2405 76
manz 0:83acd45a2405 77 // begin initialization
manz 0:83acd45a2405 78 blePeripheral.begin();
manz 0:83acd45a2405 79 /*---------------------------------------------------------------*/
manz 0:83acd45a2405 80
manz 0:83acd45a2405 81 //return value for move functions
manz 0:83acd45a2405 82 int ret;
manz 0:83acd45a2405 83
manz 3:63aef644e6d2 84 int width, length;
manz 3:63aef644e6d2 85
manz 0:83acd45a2405 86 serial.printf("BLE UART Peripheral begin!\r\n");
manz 0:83acd45a2405 87
manz 0:83acd45a2405 88 while(1)
manz 0:83acd45a2405 89 {
manz 0:83acd45a2405 90 BLECentral central = blePeripheral.central();
manz 0:83acd45a2405 91
manz 0:83acd45a2405 92 if (central)
manz 0:83acd45a2405 93 {
manz 0:83acd45a2405 94 // central connected to peripheral
manz 0:83acd45a2405 95 serial.printf("Connected to central\r\n");
manz 0:83acd45a2405 96
manz 0:83acd45a2405 97 while (central.connected())
manz 0:83acd45a2405 98 {
manz 0:83acd45a2405 99 // central still connected to peripheral
manz 0:83acd45a2405 100 if (rxCharacteristic.written())
manz 0:83acd45a2405 101 {
manz 0:83acd45a2405 102 unsigned char rxlen = rxCharacteristic.valueLength();
manz 0:83acd45a2405 103 const unsigned char *val = rxCharacteristic.value();
manz 3:63aef644e6d2 104 serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen);
manz 3:63aef644e6d2 105
manz 0:83acd45a2405 106 unsigned char i = 0;
manz 0:83acd45a2405 107 while(i<rxlen)
manz 0:83acd45a2405 108 {
manz 0:83acd45a2405 109 serial.printf("%d, ",val[i++]);
manz 0:83acd45a2405 110 }
manz 3:63aef644e6d2 111
manz 0:83acd45a2405 112 serial.printf("\r\n");
manz 3:63aef644e6d2 113 //determine mode of signal
manz 3:63aef644e6d2 114 if(rxlen == 1){
manz 3:63aef644e6d2 115 if (mode != 13){
manz 3:63aef644e6d2 116 mode = (int) val[0];
manz 3:63aef644e6d2 117 }
manz 0:83acd45a2405 118 }
manz 3:63aef644e6d2 119 if(rxlen == 4){
manz 3:63aef644e6d2 120 //check if drawing mode
manz 3:63aef644e6d2 121 if(mode != 4){
manz 3:63aef644e6d2 122 mode = 9;
manz 3:63aef644e6d2 123 }
manz 3:63aef644e6d2 124 x = (int) val[0];
manz 3:63aef644e6d2 125 y = (int) val[1];
manz 3:63aef644e6d2 126 x_dir = (int) val[2];
manz 3:63aef644e6d2 127 y_dir = (int) val[3];
manz 3:63aef644e6d2 128
manz 3:63aef644e6d2 129 // see if values are negative
manz 3:63aef644e6d2 130 if(x_dir == 1){
manz 3:63aef644e6d2 131 x = -1*x;
manz 3:63aef644e6d2 132 }
manz 3:63aef644e6d2 133 if(y_dir == 1){
manz 3:63aef644e6d2 134 y = -1*y;
manz 3:63aef644e6d2 135 }
manz 3:63aef644e6d2 136 if (rxlen == 1){
manz 3:63aef644e6d2 137 mode = (int) val[0];
manz 3:63aef644e6d2 138 }
manz 3:63aef644e6d2 139 // putting values into array
manz 3:63aef644e6d2 140 serial.printf("try to put coordinates \r\n");
manz 3:63aef644e6d2 141 one_slot.wait();
manz 3:63aef644e6d2 142 if (counter == size){
manz 3:63aef644e6d2 143 counter = 0;
manz 3:63aef644e6d2 144 }
manz 3:63aef644e6d2 145 path_x[counter] = x;
manz 3:63aef644e6d2 146 path_y[counter] = y;
manz 3:63aef644e6d2 147 counter++;
manz 3:63aef644e6d2 148 one_slot.release();
manz 3:63aef644e6d2 149 serial.printf("put coordinates \r\n");
manz 0:83acd45a2405 150 }
manz 0:83acd45a2405 151 Thread::wait(100);
manz 0:83acd45a2405 152 }
manz 0:83acd45a2405 153
manz 0:83acd45a2405 154 if(serial.readable())
manz 0:83acd45a2405 155 {
manz 0:83acd45a2405 156 if(!count_on)
manz 0:83acd45a2405 157 {
manz 0:83acd45a2405 158 count_on = 1;
manz 0:83acd45a2405 159 }
manz 0:83acd45a2405 160 interval = 0;
manz 0:83acd45a2405 161 txbuf[txlen] = serial.getc();
manz 0:83acd45a2405 162 txlen++;
manz 0:83acd45a2405 163 }
manz 0:83acd45a2405 164
manz 0:83acd45a2405 165 if(count_on) // Count the interval after receiving a new char from terminate
manz 0:83acd45a2405 166 {
manz 0:83acd45a2405 167 interval++;
manz 0:83acd45a2405 168 }
manz 0:83acd45a2405 169
manz 0:83acd45a2405 170 if(interval == 10) // If there is no char available last the interval, send the received chars to central.
manz 0:83acd45a2405 171 {
manz 0:83acd45a2405 172 interval = 0;
manz 0:83acd45a2405 173 count_on = 0;
manz 0:83acd45a2405 174 serial.printf("Received from terminal: %d bytes\r\n", txlen);
manz 0:83acd45a2405 175 txCharacteristic.setValue((const unsigned char *)txbuf, txlen);
manz 0:83acd45a2405 176 txlen = 0;
manz 0:83acd45a2405 177 }
manz 0:83acd45a2405 178 }
manz 0:83acd45a2405 179 // central disconnected
manz 0:83acd45a2405 180 serial.printf("Disconnected from central\r\n");
manz 0:83acd45a2405 181 }
manz 0:83acd45a2405 182 }
manz 0:83acd45a2405 183 }
manz 0:83acd45a2405 184
manz 3:63aef644e6d2 185 // incomplete
manz 3:63aef644e6d2 186 int draw_circle(double radius){
manz 3:63aef644e6d2 187 float currAngle = L.getAngle();
manz 3:63aef644e6d2 188 if(radius >= 5){
manz 3:63aef644e6d2 189 int outer = (int) radius*0.6;
manz 3:63aef644e6d2 190 int inner = (int) radius*0.2;
manz 3:63aef644e6d2 191 }
manz 3:63aef644e6d2 192 }
manz 3:63aef644e6d2 193
manz 0:83acd45a2405 194
manz 0:83acd45a2405 195 void move_thread(void const *args){
manz 0:83acd45a2405 196 int angle;
manz 3:63aef644e6d2 197 double length = 8;
manz 3:63aef644e6d2 198 double width = 8;
manz 3:63aef644e6d2 199 int radius = 5;
manz 0:83acd45a2405 200
manz 0:83acd45a2405 201 Timer t;
manz 0:83acd45a2405 202 t.start();
manz 3:63aef644e6d2 203
manz 0:83acd45a2405 204 L.reset();
manz 0:83acd45a2405 205 //initally put pen down
manz 0:83acd45a2405 206 //L.servo(0);
manz 0:83acd45a2405 207
manz 0:83acd45a2405 208 float currentAngle;
manz 0:83acd45a2405 209 float targetAngle;
manz 0:83acd45a2405 210 float startAngle;
manz 0:83acd45a2405 211 float diffAngle;
manz 0:83acd45a2405 212
manz 0:83acd45a2405 213 int control;
manz 3:63aef644e6d2 214 int shape_count = 0;
manz 3:63aef644e6d2 215
manz 3:63aef644e6d2 216 double draw_corr;
manz 0:83acd45a2405 217
manz 0:83acd45a2405 218 double dot,a2,b2,c2;
manz 0:83acd45a2405 219 double x_tar,y_tar;
manz 3:63aef644e6d2 220 double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr;
manz 0:83acd45a2405 221 double cosg,gamma,distance,wait_t;
manz 0:83acd45a2405 222 double x_taro = 0;
manz 0:83acd45a2405 223 double y_taro = 0;
manz 0:83acd45a2405 224 serial.printf("Started move thread\r\n");
manz 0:83acd45a2405 225 int newval = 0;
manz 0:83acd45a2405 226
manz 0:83acd45a2405 227 while(1){
manz 3:63aef644e6d2 228 // check what mode is present
manz 3:63aef644e6d2 229 //rectangle/square
manz 3:63aef644e6d2 230 if(mode == 0){
manz 3:63aef644e6d2 231 serial.printf("Draw rectangle \r\n");
manz 3:63aef644e6d2 232 //save old values and set initial
manz 3:63aef644e6d2 233 x_pen_pr = x_pen;
manz 3:63aef644e6d2 234 y_pen_pr = y_pen;
manz 3:63aef644e6d2 235 x_cent_pr = x_cent;
manz 3:63aef644e6d2 236 y_cent_pr = y_cent;
manz 3:63aef644e6d2 237 x_pen = 0;
manz 3:63aef644e6d2 238 y_pen = 0;
manz 3:63aef644e6d2 239 x_cent = 0;
manz 3:63aef644e6d2 240 y_cent = -1;
manz 3:63aef644e6d2 241
manz 3:63aef644e6d2 242 //set values
manz 3:63aef644e6d2 243 shape_x[3] = 0;
manz 3:63aef644e6d2 244 shape_y[3] = length;
manz 3:63aef644e6d2 245 shape_x[2] = width;
manz 3:63aef644e6d2 246 shape_y[2] = length;
manz 3:63aef644e6d2 247 shape_x[1] = width;
manz 3:63aef644e6d2 248 shape_y[1] = 0;
manz 3:63aef644e6d2 249 shape_x[0] = 0;
manz 3:63aef644e6d2 250 shape_y[0] = 0;
manz 3:63aef644e6d2 251 shape_count = 4;
manz 3:63aef644e6d2 252
manz 3:63aef644e6d2 253 mode = 13;
manz 3:63aef644e6d2 254 }
manz 3:63aef644e6d2 255 else if(mode == 1){
manz 3:63aef644e6d2 256 serial.printf("Draw circle \r\n");
manz 3:63aef644e6d2 257 //call circle function
manz 3:63aef644e6d2 258 control = draw_circle(radius);
manz 3:63aef644e6d2 259
manz 3:63aef644e6d2 260 mode = -1;
manz 3:63aef644e6d2 261 }
manz 3:63aef644e6d2 262 else if(mode == 2){
manz 3:63aef644e6d2 263 serial.printf("Draw triangle \r\n");
manz 3:63aef644e6d2 264 //save old values and set initial
manz 3:63aef644e6d2 265 x_pen_pr = x_pen;
manz 3:63aef644e6d2 266 y_pen_pr = y_pen;
manz 3:63aef644e6d2 267 x_cent_pr = x_cent;
manz 3:63aef644e6d2 268 y_cent_pr = y_cent;
manz 3:63aef644e6d2 269 x_pen = 0;
manz 3:63aef644e6d2 270 y_pen = 0;
manz 3:63aef644e6d2 271 x_cent = 0;
manz 3:63aef644e6d2 272 y_cent = -1;
manz 3:63aef644e6d2 273
manz 3:63aef644e6d2 274 //set values
manz 3:63aef644e6d2 275 shape_x[2] = 0;
manz 3:63aef644e6d2 276 shape_y[2] = length;
manz 3:63aef644e6d2 277 shape_x[1] = width;
manz 3:63aef644e6d2 278 shape_y[1] = 0;
manz 3:63aef644e6d2 279 shape_x[0] = 0;
manz 3:63aef644e6d2 280 shape_y[0] = 0;
manz 3:63aef644e6d2 281
manz 3:63aef644e6d2 282 shape_count = 3;
manz 3:63aef644e6d2 283 mode = 13;
manz 3:63aef644e6d2 284 }
manz 3:63aef644e6d2 285 else if(mode == 3){
manz 3:63aef644e6d2 286 serial.printf("Reset \r\n");
manz 3:63aef644e6d2 287 //set initial
manz 3:63aef644e6d2 288 x_pen = 0;
manz 3:63aef644e6d2 289 y_pen = 0;
manz 3:63aef644e6d2 290 x_cent = 0;
manz 3:63aef644e6d2 291 y_cent = -1;
manz 3:63aef644e6d2 292
manz 3:63aef644e6d2 293 mode = -1;
manz 3:63aef644e6d2 294 }
manz 3:63aef644e6d2 295 else if (mode == 4){
manz 3:63aef644e6d2 296 serial.printf("Draw freely \r\n");
manz 3:63aef644e6d2 297 draw_corr = 0.2;
manz 3:63aef644e6d2 298 }
manz 3:63aef644e6d2 299 else if (mode == 9){
manz 3:63aef644e6d2 300 serial.printf("Draw coordinates \r\n");
manz 3:63aef644e6d2 301 draw_corr = 1;
manz 3:63aef644e6d2 302
manz 3:63aef644e6d2 303 mode = -1;
manz 3:63aef644e6d2 304 }
manz 3:63aef644e6d2 305
manz 3:63aef644e6d2 306 // next coordinate is free drawing or coordinates
manz 3:63aef644e6d2 307 if(shape_count == 0){
manz 3:63aef644e6d2 308 one_slot.wait();
manz 3:63aef644e6d2 309 if(counter != start){
manz 3:63aef644e6d2 310 if (start == size){
manz 3:63aef644e6d2 311 start = 0;
manz 3:63aef644e6d2 312 }
manz 3:63aef644e6d2 313 x_tar = path_x[start];
manz 3:63aef644e6d2 314 y_tar = path_y[start];
manz 3:63aef644e6d2 315 start++;
manz 3:63aef644e6d2 316 newval = 1;
manz 0:83acd45a2405 317 }
manz 3:63aef644e6d2 318 one_slot.release();
manz 0:83acd45a2405 319 }
manz 3:63aef644e6d2 320 // next coordinate is shape
manz 3:63aef644e6d2 321 else{
manz 3:63aef644e6d2 322 shape_count = shape_count - 1;
manz 3:63aef644e6d2 323 x_tar = shape_x[shape_count];
manz 3:63aef644e6d2 324 y_tar = shape_y[shape_count];
manz 3:63aef644e6d2 325 newval = 1;
manz 3:63aef644e6d2 326 }
manz 0:83acd45a2405 327 if(newval == 1){
manz 0:83acd45a2405 328 serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar);
manz 0:83acd45a2405 329 newval = 0;
manz 0:83acd45a2405 330
manz 0:83acd45a2405 331 //compute angle and turn direction
manz 0:83acd45a2405 332 a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent);
manz 0:83acd45a2405 333 b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen);
manz 0:83acd45a2405 334 c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent);
manz 0:83acd45a2405 335 cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2));
manz 0:83acd45a2405 336 gamma = acos(cosg)/3.14159*180;
manz 0:83acd45a2405 337
manz 0:83acd45a2405 338 dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent);
manz 0:83acd45a2405 339
manz 0:83acd45a2405 340 serial.printf("Angle: %f \r\n",gamma);
manz 0:83acd45a2405 341 angle = ceil(180 - gamma);
manz 0:83acd45a2405 342 serial.printf("Turning angle: %i \r\n",angle);
manz 0:83acd45a2405 343
manz 0:83acd45a2405 344 //put pen up
manz 0:83acd45a2405 345 //L.servo(30);
manz 0:83acd45a2405 346
manz 0:83acd45a2405 347 currentAngle = L.getAngle();
manz 0:83acd45a2405 348 if(dot > 0){
manz 0:83acd45a2405 349 //serial.printf("Turn right \r\n");
manz 0:83acd45a2405 350 targetAngle = fmod(currentAngle+angle,360);
manz 0:83acd45a2405 351
manz 0:83acd45a2405 352 while(fmod(abs(L.getAngle() - targetAngle),360)> 3){
manz 0:83acd45a2405 353 control = step_right();
manz 0:83acd45a2405 354 serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle));
manz 0:83acd45a2405 355 }
manz 0:83acd45a2405 356 //serial.printf("Reached target \r\n");
manz 0:83acd45a2405 357 }
manz 0:83acd45a2405 358 else if(dot < 0){
manz 0:83acd45a2405 359 //serial.printf("Turn left \r\n");
manz 0:83acd45a2405 360 targetAngle = fmod(currentAngle-angle,360);
manz 0:83acd45a2405 361 while(fmod(abs(L.getAngle() - targetAngle),360)> 3){
manz 0:83acd45a2405 362 control = step_left();
manz 0:83acd45a2405 363 serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle));
manz 0:83acd45a2405 364 }
manz 0:83acd45a2405 365 }
manz 0:83acd45a2405 366 else{
manz 0:83acd45a2405 367 }
manz 0:83acd45a2405 368 //put pen down again
manz 0:83acd45a2405 369 //L.servo(0);
manz 0:83acd45a2405 370
manz 0:83acd45a2405 371 startAngle = L.getAngle();
manz 0:83acd45a2405 372 serial.printf("Current angle: %f \r\n",startAngle);
manz 0:83acd45a2405 373 //compute length of path til target
manz 0:83acd45a2405 374 distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen));
manz 0:83acd45a2405 375
manz 0:83acd45a2405 376 //forward SCRIBE til target
manz 0:83acd45a2405 377 wait_t = distance/speed*t_factor;
manz 0:83acd45a2405 378 t.reset();
manz 0:83acd45a2405 379 while(t.read() < wait_t){
manz 0:83acd45a2405 380 control = step_f();
manz 0:83acd45a2405 381 }
manz 0:83acd45a2405 382 serial.printf("Reached destination \r\n");
manz 0:83acd45a2405 383
manz 0:83acd45a2405 384
manz 0:83acd45a2405 385 //update pen and center when at target
manz 0:83acd45a2405 386 x_pen = x_tar;
manz 0:83acd45a2405 387 y_pen = y_tar;
manz 0:83acd45a2405 388 serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen);
manz 0:83acd45a2405 389 x_cent = x_taro;
manz 0:83acd45a2405 390 y_cent = y_taro;
manz 0:83acd45a2405 391 serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent);
manz 0:83acd45a2405 392 x_taro = x_tar;
manz 0:83acd45a2405 393 y_taro = y_tar;
manz 0:83acd45a2405 394
manz 0:83acd45a2405 395 }
manz 0:83acd45a2405 396 Thread::wait(100);
manz 0:83acd45a2405 397 }
manz 0:83acd45a2405 398 }
manz 0:83acd45a2405 399
manz 0:83acd45a2405 400 int main()
manz 0:83acd45a2405 401 {
manz 0:83acd45a2405 402 serial.printf("Starting the threads");
manz 0:83acd45a2405 403
manz 0:83acd45a2405 404 Thread bluetooth(bluetooth_thread);
manz 0:83acd45a2405 405 Thread move(move_thread);
manz 0:83acd45a2405 406
manz 0:83acd45a2405 407 Thread::wait(osWaitForever);
manz 0:83acd45a2405 408
manz 0:83acd45a2405 409 }