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@5:1da4d4050306, 2016-04-25 (annotated)

Committer:
manz
Date:
Mon Apr 25 05:38:48 2016 +0000
Revision:
5:1da4d4050306
Parent:
3:63aef644e6d2
Child:
7:1bb3b5b66fe8
working with new thread system

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 5:1da4d4050306 7 #include "rtos.h"
manz 5:1da4d4050306 8 #include "localization.h"
manz 0:83acd45a2405 9 #include "stepper.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 5:1da4d4050306 34 int which_thread = 0;
manz 5:1da4d4050306 35
manz 0:83acd45a2405 36 // array to save the values of the user
manz 5:1da4d4050306 37 const int size = 100;
manz 0:83acd45a2405 38 int path_x [size];
manz 0:83acd45a2405 39 int path_y [size];
manz 5:1da4d4050306 40 int path_p [size];
manz 0:83acd45a2405 41 int counter = 0;
manz 0:83acd45a2405 42 int start = 0;
manz 0:83acd45a2405 43
manz 3:63aef644e6d2 44 const int shape_size = 10;
manz 3:63aef644e6d2 45 int shape_x [shape_size];
manz 3:63aef644e6d2 46 int shape_y [shape_size];
manz 3:63aef644e6d2 47
manz 5:1da4d4050306 48 int mode = -1; //default mode is idle
manz 5:1da4d4050306 49 int pmode = 1; //default pen is down
manz 3:63aef644e6d2 50
manz 0:83acd45a2405 51 //coordinates of the pen and the centre of SCRIBE at beginning
manz 0:83acd45a2405 52 double x_pen = 0;
manz 0:83acd45a2405 53 double y_pen = 0;
manz 0:83acd45a2405 54 double x_cent = 0;
manz 0:83acd45a2405 55 double y_cent = -1;
manz 0:83acd45a2405 56
manz 0:83acd45a2405 57 //speed of SCRIBE
manz 0:83acd45a2405 58 float speed = 0.3;
manz 0:83acd45a2405 59 float t_factor = 0.15;
manz 0:83acd45a2405 60
manz 0:83acd45a2405 61
manz 0:83acd45a2405 62 Semaphore one_slot(1);
manz 0:83acd45a2405 63
manz 3:63aef644e6d2 64 localization L;
manz 0:83acd45a2405 65
manz 5:1da4d4050306 66 osThreadId bluetooth_id, mover_id;
manz 5:1da4d4050306 67
manz 0:83acd45a2405 68 void bluetooth_thread(void const *args){
manz 5:1da4d4050306 69 bluetooth_id = Thread::gettid();
manz 5:1da4d4050306 70
manz 0:83acd45a2405 71 serial.printf("Serial begin!\r\n");
manz 5:1da4d4050306 72
manz 5:1da4d4050306 73 FILE *fp = fopen("/local/trial.m", "w");
manz 5:1da4d4050306 74 if (fp == NULL) {
manz 5:1da4d4050306 75 serial.printf("ERROR: File open!\r\n");
manz 5:1da4d4050306 76 }
manz 5:1da4d4050306 77
manz 0:83acd45a2405 78 int x,y,x_dir,y_dir;
manz 0:83acd45a2405 79
manz 0:83acd45a2405 80 /*----- BLE Utility ---------------------------------------------*/
manz 0:83acd45a2405 81 // set advertised local name and service UUID
manz 0:83acd45a2405 82 blePeripheral.setLocalName("BLE Shield");
manz 0:83acd45a2405 83
manz 0:83acd45a2405 84 blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
manz 0:83acd45a2405 85
manz 0:83acd45a2405 86 // add service and characteristic
manz 0:83acd45a2405 87 blePeripheral.addAttribute(uartService);
manz 0:83acd45a2405 88 blePeripheral.addAttribute(rxCharacteristic);
manz 0:83acd45a2405 89 blePeripheral.addAttribute(txCharacteristic);
manz 0:83acd45a2405 90
manz 0:83acd45a2405 91 // begin initialization
manz 0:83acd45a2405 92 blePeripheral.begin();
manz 0:83acd45a2405 93 /*---------------------------------------------------------------*/
manz 0:83acd45a2405 94
manz 0:83acd45a2405 95 serial.printf("BLE UART Peripheral begin!\r\n");
manz 0:83acd45a2405 96
manz 0:83acd45a2405 97 while(1)
manz 0:83acd45a2405 98 {
manz 0:83acd45a2405 99 BLECentral central = blePeripheral.central();
manz 0:83acd45a2405 100
manz 0:83acd45a2405 101 if (central)
manz 0:83acd45a2405 102 {
manz 0:83acd45a2405 103 // central connected to peripheral
manz 0:83acd45a2405 104 serial.printf("Connected to central\r\n");
manz 0:83acd45a2405 105 while (central.connected())
manz 5:1da4d4050306 106 {
manz 5:1da4d4050306 107 Thread::signal_wait(0x1);
manz 0:83acd45a2405 108 // central still connected to peripheral
manz 0:83acd45a2405 109 if (rxCharacteristic.written())
manz 0:83acd45a2405 110 {
manz 0:83acd45a2405 111 unsigned char rxlen = rxCharacteristic.valueLength();
manz 0:83acd45a2405 112 const unsigned char *val = rxCharacteristic.value();
manz 5:1da4d4050306 113 /*serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen);
manz 3:63aef644e6d2 114
manz 0:83acd45a2405 115 unsigned char i = 0;
manz 0:83acd45a2405 116 while(i<rxlen)
manz 0:83acd45a2405 117 {
manz 0:83acd45a2405 118 serial.printf("%d, ",val[i++]);
manz 0:83acd45a2405 119 }
manz 3:63aef644e6d2 120
manz 5:1da4d4050306 121 serial.printf("\r\n");*/
manz 3:63aef644e6d2 122 //determine mode of signal
manz 3:63aef644e6d2 123 if(rxlen == 1){
manz 5:1da4d4050306 124 // inputs blocked until shape finished
manz 3:63aef644e6d2 125 if (mode != 13){
manz 3:63aef644e6d2 126 mode = (int) val[0];
manz 5:1da4d4050306 127 if(mode != 4 && mode !=6){
manz 5:1da4d4050306 128 which_thread = 1;
manz 5:1da4d4050306 129 osSignalClear(bluetooth_id,0x1);
manz 5:1da4d4050306 130 }
manz 5:1da4d4050306 131 }
manz 0:83acd45a2405 132 }
manz 5:1da4d4050306 133 if (mode == 6){
manz 5:1da4d4050306 134 serial.printf("Storing drawn coordinates\r\n");
manz 5:1da4d4050306 135 serial.printf("x_coord = [");
manz 5:1da4d4050306 136 for (int i = 0; i < size; i++) {
manz 5:1da4d4050306 137 serial.printf("%i ", path_x[i]);
manz 5:1da4d4050306 138 }
manz 5:1da4d4050306 139 serial.printf("];\n");
manz 5:1da4d4050306 140 serial.printf("y_coord = [");
manz 5:1da4d4050306 141 for (int i = 0; i < size; i++) {
manz 5:1da4d4050306 142 serial.printf("%i ", path_y[i]);
manz 5:1da4d4050306 143 }
manz 5:1da4d4050306 144 serial.printf("];\n");
manz 5:1da4d4050306 145 }
manz 5:1da4d4050306 146 // stroke of pen finished
manz 5:1da4d4050306 147 if (mode == 7){
manz 5:1da4d4050306 148 serial.printf("Other threads can proceed \r\n");
manz 5:1da4d4050306 149 osSignalClear(bluetooth_id,0x1);
manz 5:1da4d4050306 150 which_thread = 1;
manz 5:1da4d4050306 151 }
manz 5:1da4d4050306 152 if(rxlen == 5){
manz 5:1da4d4050306 153 // in coordinates mode - accept negative coordinates
manz 5:1da4d4050306 154 if(mode == 6){
manz 3:63aef644e6d2 155 mode = 9;
manz 5:1da4d4050306 156 }
manz 5:1da4d4050306 157 // in draw mode - convert coordinates
manz 5:1da4d4050306 158 x = (int) val[0]*256 + val[1];
manz 5:1da4d4050306 159 y = (int) val[2]*256 + val[3];
manz 3:63aef644e6d2 160
manz 3:63aef644e6d2 161 // putting values into array
manz 5:1da4d4050306 162 //serial.printf("try to put coordinates \r\n");
manz 3:63aef644e6d2 163 one_slot.wait();
manz 3:63aef644e6d2 164 if (counter == size){
manz 5:1da4d4050306 165 serial.printf("Overwriting values \r\n");
manz 3:63aef644e6d2 166 counter = 0;
manz 3:63aef644e6d2 167 }
manz 3:63aef644e6d2 168 path_x[counter] = x;
manz 3:63aef644e6d2 169 path_y[counter] = y;
manz 5:1da4d4050306 170
manz 5:1da4d4050306 171 //check if pen needs to be up or down
manz 5:1da4d4050306 172 path_p[counter] = pmode;
manz 5:1da4d4050306 173
manz 3:63aef644e6d2 174 counter++;
manz 3:63aef644e6d2 175 one_slot.release();
manz 5:1da4d4050306 176 if(mode == 9){
manz 5:1da4d4050306 177 which_thread = 1;
manz 5:1da4d4050306 178 osSignalClear(bluetooth_id,0x1);
manz 5:1da4d4050306 179 }
manz 5:1da4d4050306 180 //serial.printf("put coordinates \r\n");
manz 0:83acd45a2405 181 }
manz 0:83acd45a2405 182 }
manz 0:83acd45a2405 183
manz 0:83acd45a2405 184 if(serial.readable())
manz 0:83acd45a2405 185 {
manz 0:83acd45a2405 186 if(!count_on)
manz 0:83acd45a2405 187 {
manz 0:83acd45a2405 188 count_on = 1;
manz 0:83acd45a2405 189 }
manz 0:83acd45a2405 190 interval = 0;
manz 0:83acd45a2405 191 txbuf[txlen] = serial.getc();
manz 0:83acd45a2405 192 txlen++;
manz 0:83acd45a2405 193 }
manz 0:83acd45a2405 194
manz 0:83acd45a2405 195 if(count_on) // Count the interval after receiving a new char from terminate
manz 0:83acd45a2405 196 {
manz 0:83acd45a2405 197 interval++;
manz 0:83acd45a2405 198 }
manz 0:83acd45a2405 199
manz 0:83acd45a2405 200 if(interval == 10) // If there is no char available last the interval, send the received chars to central.
manz 0:83acd45a2405 201 {
manz 0:83acd45a2405 202 interval = 0;
manz 0:83acd45a2405 203 count_on = 0;
manz 5:1da4d4050306 204 //serial.printf("Received from terminal: %d bytes\r\n", txlen);
manz 0:83acd45a2405 205 txCharacteristic.setValue((const unsigned char *)txbuf, txlen);
manz 0:83acd45a2405 206 txlen = 0;
manz 0:83acd45a2405 207 }
manz 0:83acd45a2405 208 }
manz 5:1da4d4050306 209
manz 0:83acd45a2405 210 // central disconnected
manz 0:83acd45a2405 211 serial.printf("Disconnected from central\r\n");
manz 0:83acd45a2405 212 }
manz 0:83acd45a2405 213 }
manz 0:83acd45a2405 214 }
manz 0:83acd45a2405 215
manz 5:1da4d4050306 216
manz 3:63aef644e6d2 217 // incomplete
manz 3:63aef644e6d2 218 int draw_circle(double radius){
manz 3:63aef644e6d2 219 float currAngle = L.getAngle();
manz 3:63aef644e6d2 220 if(radius >= 5){
manz 3:63aef644e6d2 221 int outer = (int) radius*0.6;
manz 3:63aef644e6d2 222 int inner = (int) radius*0.2;
manz 3:63aef644e6d2 223 }
manz 3:63aef644e6d2 224 }
manz 3:63aef644e6d2 225
manz 0:83acd45a2405 226
manz 0:83acd45a2405 227 void move_thread(void const *args){
manz 5:1da4d4050306 228
manz 0:83acd45a2405 229 int angle;
manz 5:1da4d4050306 230 double length = 40;
manz 5:1da4d4050306 231 double width = 40;
manz 3:63aef644e6d2 232 int radius = 5;
manz 0:83acd45a2405 233
manz 0:83acd45a2405 234 Timer t;
manz 0:83acd45a2405 235 t.start();
manz 3:63aef644e6d2 236
manz 0:83acd45a2405 237 L.reset();
manz 0:83acd45a2405 238 //initally put pen down
manz 0:83acd45a2405 239 //L.servo(0);
manz 0:83acd45a2405 240
manz 0:83acd45a2405 241 float currentAngle;
manz 0:83acd45a2405 242 float targetAngle;
manz 0:83acd45a2405 243 float startAngle;
manz 0:83acd45a2405 244 float diffAngle;
manz 0:83acd45a2405 245
manz 5:1da4d4050306 246 int steps;
manz 5:1da4d4050306 247
manz 0:83acd45a2405 248 int control;
manz 3:63aef644e6d2 249 int shape_count = 0;
manz 5:1da4d4050306 250 int p_mode;
manz 5:1da4d4050306 251 int first = 0;
manz 3:63aef644e6d2 252
manz 5:1da4d4050306 253 double draw_corr = 5;
manz 0:83acd45a2405 254
manz 0:83acd45a2405 255 double dot,a2,b2,c2;
manz 0:83acd45a2405 256 double x_tar,y_tar;
manz 3:63aef644e6d2 257 double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr;
manz 0:83acd45a2405 258 double cosg,gamma,distance,wait_t;
manz 0:83acd45a2405 259 double x_taro = 0;
manz 0:83acd45a2405 260 double y_taro = 0;
manz 0:83acd45a2405 261 serial.printf("Started move thread\r\n");
manz 0:83acd45a2405 262 int newval = 0;
manz 5:1da4d4050306 263
manz 0:83acd45a2405 264 while(1){
manz 3:63aef644e6d2 265 // check what mode is present
manz 5:1da4d4050306 266 serial.printf("Cylce \r\n");
manz 5:1da4d4050306 267 Thread::signal_wait(0x1);
manz 5:1da4d4050306 268 if(mode == -1){
manz 5:1da4d4050306 269 serial.printf("here ends");
manz 5:1da4d4050306 270 osSignalClear(mover_id,0x1);
manz 5:1da4d4050306 271 which_thread = 0;
manz 5:1da4d4050306 272 }
manz 3:63aef644e6d2 273 //rectangle/square
manz 5:1da4d4050306 274 else if(mode == 0){
manz 3:63aef644e6d2 275 serial.printf("Draw rectangle \r\n");
manz 3:63aef644e6d2 276 //save old values and set initial
manz 3:63aef644e6d2 277 x_pen_pr = x_pen;
manz 3:63aef644e6d2 278 y_pen_pr = y_pen;
manz 3:63aef644e6d2 279 x_cent_pr = x_cent;
manz 3:63aef644e6d2 280 y_cent_pr = y_cent;
manz 3:63aef644e6d2 281 x_pen = 0;
manz 3:63aef644e6d2 282 y_pen = 0;
manz 3:63aef644e6d2 283 x_cent = 0;
manz 3:63aef644e6d2 284 y_cent = -1;
manz 5:1da4d4050306 285 x_taro = 0;
manz 5:1da4d4050306 286 y_taro = 0;
manz 3:63aef644e6d2 287
manz 3:63aef644e6d2 288 //set values
manz 3:63aef644e6d2 289 shape_x[3] = 0;
manz 3:63aef644e6d2 290 shape_y[3] = length;
manz 3:63aef644e6d2 291 shape_x[2] = width;
manz 3:63aef644e6d2 292 shape_y[2] = length;
manz 3:63aef644e6d2 293 shape_x[1] = width;
manz 3:63aef644e6d2 294 shape_y[1] = 0;
manz 3:63aef644e6d2 295 shape_x[0] = 0;
manz 3:63aef644e6d2 296 shape_y[0] = 0;
manz 3:63aef644e6d2 297 shape_count = 4;
manz 3:63aef644e6d2 298
manz 3:63aef644e6d2 299 mode = 13;
manz 3:63aef644e6d2 300 }
manz 3:63aef644e6d2 301 else if(mode == 1){
manz 3:63aef644e6d2 302 serial.printf("Draw circle \r\n");
manz 3:63aef644e6d2 303 //call circle function
manz 3:63aef644e6d2 304 control = draw_circle(radius);
manz 3:63aef644e6d2 305
manz 3:63aef644e6d2 306 mode = -1;
manz 3:63aef644e6d2 307 }
manz 3:63aef644e6d2 308 else if(mode == 2){
manz 3:63aef644e6d2 309 serial.printf("Draw triangle \r\n");
manz 3:63aef644e6d2 310 //save old values and set initial
manz 3:63aef644e6d2 311 x_pen_pr = x_pen;
manz 3:63aef644e6d2 312 y_pen_pr = y_pen;
manz 3:63aef644e6d2 313 x_cent_pr = x_cent;
manz 3:63aef644e6d2 314 y_cent_pr = y_cent;
manz 3:63aef644e6d2 315 x_pen = 0;
manz 3:63aef644e6d2 316 y_pen = 0;
manz 3:63aef644e6d2 317 x_cent = 0;
manz 3:63aef644e6d2 318 y_cent = -1;
manz 5:1da4d4050306 319 x_taro = 0;
manz 5:1da4d4050306 320 y_taro = 0;
manz 3:63aef644e6d2 321
manz 3:63aef644e6d2 322 //set values
manz 3:63aef644e6d2 323 shape_x[2] = 0;
manz 3:63aef644e6d2 324 shape_y[2] = length;
manz 3:63aef644e6d2 325 shape_x[1] = width;
manz 3:63aef644e6d2 326 shape_y[1] = 0;
manz 3:63aef644e6d2 327 shape_x[0] = 0;
manz 3:63aef644e6d2 328 shape_y[0] = 0;
manz 3:63aef644e6d2 329
manz 3:63aef644e6d2 330 shape_count = 3;
manz 3:63aef644e6d2 331 mode = 13;
manz 3:63aef644e6d2 332 }
manz 3:63aef644e6d2 333 else if(mode == 3){
manz 3:63aef644e6d2 334 serial.printf("Reset \r\n");
manz 3:63aef644e6d2 335 //set initial
manz 3:63aef644e6d2 336 x_pen = 0;
manz 3:63aef644e6d2 337 y_pen = 0;
manz 3:63aef644e6d2 338 x_cent = 0;
manz 3:63aef644e6d2 339 y_cent = -1;
manz 3:63aef644e6d2 340
manz 3:63aef644e6d2 341 mode = -1;
manz 3:63aef644e6d2 342 }
manz 5:1da4d4050306 343 else if (mode == 7){
manz 5:1da4d4050306 344 if (first == 0){
manz 5:1da4d4050306 345 serial.printf("Draw freely \r\n");
manz 5:1da4d4050306 346 draw_corr = 1;
manz 5:1da4d4050306 347 }
manz 5:1da4d4050306 348 first = 1;
manz 5:1da4d4050306 349
manz 3:63aef644e6d2 350 }
manz 3:63aef644e6d2 351 else if (mode == 9){
manz 3:63aef644e6d2 352 serial.printf("Draw coordinates \r\n");
manz 5:1da4d4050306 353 draw_corr = 5;
manz 3:63aef644e6d2 354 mode = -1;
manz 3:63aef644e6d2 355 }
manz 3:63aef644e6d2 356
manz 3:63aef644e6d2 357 // next coordinate is free drawing or coordinates
manz 3:63aef644e6d2 358 if(shape_count == 0){
manz 3:63aef644e6d2 359 one_slot.wait();
manz 3:63aef644e6d2 360 if(counter != start){
manz 3:63aef644e6d2 361 if (start == size){
manz 3:63aef644e6d2 362 start = 0;
manz 3:63aef644e6d2 363 }
manz 5:1da4d4050306 364 x_tar = (double) path_x[start]*draw_corr;
manz 5:1da4d4050306 365 y_tar = (double) path_y[start]*draw_corr;
manz 5:1da4d4050306 366 p_mode = path_p[start];
manz 3:63aef644e6d2 367 start++;
manz 5:1da4d4050306 368 if(start == counter) mode = -1;
manz 3:63aef644e6d2 369 newval = 1;
manz 0:83acd45a2405 370 }
manz 3:63aef644e6d2 371 one_slot.release();
manz 0:83acd45a2405 372 }
manz 3:63aef644e6d2 373 // next coordinate is shape
manz 3:63aef644e6d2 374 else{
manz 3:63aef644e6d2 375 shape_count = shape_count - 1;
manz 5:1da4d4050306 376 x_tar = (double) shape_x[shape_count];
manz 5:1da4d4050306 377 y_tar = (double) shape_y[shape_count];
manz 5:1da4d4050306 378 p_mode = 1;
manz 3:63aef644e6d2 379 newval = 1;
manz 5:1da4d4050306 380
manz 5:1da4d4050306 381 //last move -> unblock input
manz 5:1da4d4050306 382 if(shape_count == 0){
manz 5:1da4d4050306 383 mode = -1;
manz 5:1da4d4050306 384 }
manz 3:63aef644e6d2 385 }
manz 0:83acd45a2405 386 if(newval == 1){
manz 0:83acd45a2405 387 serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar);
manz 0:83acd45a2405 388 newval = 0;
manz 0:83acd45a2405 389
manz 0:83acd45a2405 390 //compute angle and turn direction
manz 0:83acd45a2405 391 a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent);
manz 0:83acd45a2405 392 b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen);
manz 0:83acd45a2405 393 c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent);
manz 0:83acd45a2405 394 cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2));
manz 0:83acd45a2405 395 gamma = acos(cosg)/3.14159*180;
manz 0:83acd45a2405 396
manz 0:83acd45a2405 397 dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent);
manz 0:83acd45a2405 398
manz 5:1da4d4050306 399 //serial.printf("Angle: %f \r\n",gamma);
manz 0:83acd45a2405 400 angle = ceil(180 - gamma);
manz 0:83acd45a2405 401 serial.printf("Turning angle: %i \r\n",angle);
manz 0:83acd45a2405 402
manz 5:1da4d4050306 403 //put pen down
manz 5:1da4d4050306 404 if (p_mode == 1){
manz 5:1da4d4050306 405 //serial.printf("Pen down \r\n");
manz 5:1da4d4050306 406 L.servo(100);
manz 5:1da4d4050306 407 }
manz 0:83acd45a2405 408 //put pen up
manz 5:1da4d4050306 409 else if (p_mode == 0){
manz 5:1da4d4050306 410 //serial.printf("Pen up \r\n");
manz 5:1da4d4050306 411 L.servo(90);
manz 5:1da4d4050306 412 }
manz 0:83acd45a2405 413
manz 0:83acd45a2405 414 currentAngle = L.getAngle();
manz 0:83acd45a2405 415 if(dot > 0){
manz 0:83acd45a2405 416 //serial.printf("Turn right \r\n");
manz 0:83acd45a2405 417 targetAngle = fmod(currentAngle+angle,360);
manz 0:83acd45a2405 418
manz 0:83acd45a2405 419 while(fmod(abs(L.getAngle() - targetAngle),360)> 3){
manz 0:83acd45a2405 420 control = step_right();
manz 5:1da4d4050306 421 //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360));
manz 0:83acd45a2405 422 }
manz 0:83acd45a2405 423 //serial.printf("Reached target \r\n");
manz 0:83acd45a2405 424 }
manz 0:83acd45a2405 425 else if(dot < 0){
manz 0:83acd45a2405 426 //serial.printf("Turn left \r\n");
manz 0:83acd45a2405 427 targetAngle = fmod(currentAngle-angle,360);
manz 0:83acd45a2405 428 while(fmod(abs(L.getAngle() - targetAngle),360)> 3){
manz 0:83acd45a2405 429 control = step_left();
manz 5:1da4d4050306 430 //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360));
manz 0:83acd45a2405 431 }
manz 0:83acd45a2405 432 }
manz 0:83acd45a2405 433 //compute length of path til target
manz 0:83acd45a2405 434 distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen));
manz 0:83acd45a2405 435
manz 0:83acd45a2405 436 //forward SCRIBE til target
manz 5:1da4d4050306 437 steps = (int) distance;
manz 5:1da4d4050306 438 for(int i = 0; i< steps; i++){
manz 0:83acd45a2405 439 control = step_f();
manz 0:83acd45a2405 440 }
manz 5:1da4d4050306 441 //serial.printf("Reached destination \r\n");
manz 0:83acd45a2405 442
manz 0:83acd45a2405 443 //update pen and center when at target
manz 0:83acd45a2405 444 x_pen = x_tar;
manz 0:83acd45a2405 445 y_pen = y_tar;
manz 0:83acd45a2405 446 serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen);
manz 0:83acd45a2405 447 x_cent = x_taro;
manz 0:83acd45a2405 448 y_cent = y_taro;
manz 0:83acd45a2405 449 serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent);
manz 0:83acd45a2405 450 x_taro = x_tar;
manz 0:83acd45a2405 451 y_taro = y_tar;
manz 0:83acd45a2405 452
manz 5:1da4d4050306 453 }
manz 0:83acd45a2405 454 }
manz 0:83acd45a2405 455 }
manz 0:83acd45a2405 456
manz 5:1da4d4050306 457
manz 5:1da4d4050306 458
manz 0:83acd45a2405 459 int main()
manz 0:83acd45a2405 460 {
manz 0:83acd45a2405 461 serial.printf("Starting the threads");
manz 0:83acd45a2405 462
manz 0:83acd45a2405 463 Thread bluetooth(bluetooth_thread);
manz 0:83acd45a2405 464 Thread move(move_thread);
manz 5:1da4d4050306 465 mover_id = move.gettid();
manz 5:1da4d4050306 466 bluetooth_id = bluetooth.gettid();
manz 0:83acd45a2405 467
manz 5:1da4d4050306 468 while(1){
manz 5:1da4d4050306 469 if(which_thread == 0) bluetooth.signal_set(0x1);
manz 5:1da4d4050306 470 else move.signal_set(0x1);
manz 5:1da4d4050306 471 }
manz 0:83acd45a2405 472 }