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@9:946d400b2f13, 2016-05-05 (annotated)

Committer:
manz
Date:
Thu May 05 21:52:50 2016 +0000
Revision:
9:946d400b2f13
Parent:
8:4d7b2dbdb694
Child:
12:01250ea24795
wheel encoder added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manz 9:946d400b2f13 1 // main of SCRIBE servo
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 7:1bb3b5b66fe8 9 #include "servo.h"
manz 0:83acd45a2405 10
manz 0:83acd45a2405 11 Serial serial(USBTX, USBRX);
manz 7:1bb3b5b66fe8 12
manz 9:946d400b2f13 13 //Analog In for ADC A0 -> p15
manz 9:946d400b2f13 14 AnalogIn ain1(A0);
manz 9:946d400b2f13 15 AnalogIn ain2(A1);
manz 7:1bb3b5b66fe8 16
manz 7:1bb3b5b66fe8 17 //Digital LED
manz 7:1bb3b5b66fe8 18 DigitalOut led1(LED1);
manz 7:1bb3b5b66fe8 19 DigitalOut led2(LED2);
manz 7:1bb3b5b66fe8 20 DigitalOut led3(LED3);
manz 7:1bb3b5b66fe8 21 DigitalOut led4(LED4);
manz 7:1bb3b5b66fe8 22
manz 7:1bb3b5b66fe8 23
manz 0:83acd45a2405 24 SPI spi(p11, p12, p13);
manz 0:83acd45a2405 25 DigitalInOut BLE_RDY(p21);
manz 0:83acd45a2405 26 DigitalInOut BLE_REQ(p22);
manz 0:83acd45a2405 27 DigitalInOut BLE_RESET(p23);
manz 0:83acd45a2405 28
manz 0:83acd45a2405 29 unsigned char txbuf[16] = {0};
manz 0:83acd45a2405 30 unsigned char txlen = 0;
manz 0:83acd45a2405 31
manz 0:83acd45a2405 32 // create peripheral
manz 0:83acd45a2405 33 BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET);
manz 0:83acd45a2405 34
manz 0:83acd45a2405 35 // create service w/ uuid
manz 0:83acd45a2405 36 BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e");
manz 0:83acd45a2405 37
manz 0:83acd45a2405 38 // create characteristics
manz 0:83acd45a2405 39 BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
manz 0:83acd45a2405 40 BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
manz 0:83acd45a2405 41
manz 0:83acd45a2405 42 unsigned int interval = 0;
manz 0:83acd45a2405 43 unsigned char count_on = 0;
manz 0:83acd45a2405 44
manz 5:1da4d4050306 45 int which_thread = 0;
manz 5:1da4d4050306 46
manz 0:83acd45a2405 47 // array to save the values of the user
manz 7:1bb3b5b66fe8 48 const int size = 500;
manz 0:83acd45a2405 49 int path_x [size];
manz 0:83acd45a2405 50 int path_y [size];
manz 5:1da4d4050306 51 int path_p [size];
manz 0:83acd45a2405 52 int counter = 0;
manz 0:83acd45a2405 53 int start = 0;
manz 0:83acd45a2405 54
manz 7:1bb3b5b66fe8 55
manz 3:63aef644e6d2 56 const int shape_size = 10;
manz 7:1bb3b5b66fe8 57 double shape_x [shape_size];
manz 7:1bb3b5b66fe8 58 double shape_y [shape_size];
manz 3:63aef644e6d2 59
manz 5:1da4d4050306 60 int mode = -1; //default mode is idle
manz 5:1da4d4050306 61 int pmode = 1; //default pen is down
manz 3:63aef644e6d2 62
manz 0:83acd45a2405 63 //coordinates of the pen and the centre of SCRIBE at beginning
manz 0:83acd45a2405 64 double x_pen = 0;
manz 0:83acd45a2405 65 double y_pen = 0;
manz 0:83acd45a2405 66 double x_cent = 0;
manz 0:83acd45a2405 67 double y_cent = -1;
manz 0:83acd45a2405 68
manz 0:83acd45a2405 69 //speed of SCRIBE
manz 0:83acd45a2405 70 float speed = 0.3;
manz 0:83acd45a2405 71 float t_factor = 0.15;
manz 0:83acd45a2405 72
manz 0:83acd45a2405 73
manz 0:83acd45a2405 74 Semaphore one_slot(1);
manz 0:83acd45a2405 75
manz 3:63aef644e6d2 76 localization L;
manz 0:83acd45a2405 77
manz 5:1da4d4050306 78 osThreadId bluetooth_id, mover_id;
manz 5:1da4d4050306 79
manz 0:83acd45a2405 80 void bluetooth_thread(void const *args){
manz 5:1da4d4050306 81 bluetooth_id = Thread::gettid();
manz 5:1da4d4050306 82
manz 0:83acd45a2405 83 serial.printf("Serial begin!\r\n");
manz 5:1da4d4050306 84
manz 5:1da4d4050306 85 FILE *fp = fopen("/local/trial.m", "w");
manz 5:1da4d4050306 86 if (fp == NULL) {
manz 5:1da4d4050306 87 serial.printf("ERROR: File open!\r\n");
manz 5:1da4d4050306 88 }
manz 5:1da4d4050306 89
manz 0:83acd45a2405 90 int x,y,x_dir,y_dir;
manz 7:1bb3b5b66fe8 91 int stroke = 0;
manz 0:83acd45a2405 92
manz 0:83acd45a2405 93 /*----- BLE Utility ---------------------------------------------*/
manz 0:83acd45a2405 94 // set advertised local name and service UUID
manz 0:83acd45a2405 95 blePeripheral.setLocalName("BLE Shield");
manz 0:83acd45a2405 96
manz 0:83acd45a2405 97 blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
manz 0:83acd45a2405 98
manz 0:83acd45a2405 99 // add service and characteristic
manz 0:83acd45a2405 100 blePeripheral.addAttribute(uartService);
manz 0:83acd45a2405 101 blePeripheral.addAttribute(rxCharacteristic);
manz 0:83acd45a2405 102 blePeripheral.addAttribute(txCharacteristic);
manz 0:83acd45a2405 103
manz 0:83acd45a2405 104 // begin initialization
manz 0:83acd45a2405 105 blePeripheral.begin();
manz 0:83acd45a2405 106 /*---------------------------------------------------------------*/
manz 0:83acd45a2405 107
manz 0:83acd45a2405 108 serial.printf("BLE UART Peripheral begin!\r\n");
manz 0:83acd45a2405 109
manz 0:83acd45a2405 110 while(1)
manz 0:83acd45a2405 111 {
manz 0:83acd45a2405 112 BLECentral central = blePeripheral.central();
manz 7:1bb3b5b66fe8 113
manz 0:83acd45a2405 114 if (central)
manz 0:83acd45a2405 115 {
manz 0:83acd45a2405 116 // central connected to peripheral
manz 0:83acd45a2405 117 serial.printf("Connected to central\r\n");
manz 0:83acd45a2405 118 while (central.connected())
manz 5:1da4d4050306 119 {
manz 7:1bb3b5b66fe8 120 //serial.printf("*** Connected to bluetooth \r\n");
manz 7:1bb3b5b66fe8 121 Thread::wait(1);
manz 0:83acd45a2405 122 // central still connected to peripheral
manz 0:83acd45a2405 123 if (rxCharacteristic.written())
manz 0:83acd45a2405 124 {
manz 0:83acd45a2405 125 unsigned char rxlen = rxCharacteristic.valueLength();
manz 0:83acd45a2405 126 const unsigned char *val = rxCharacteristic.value();
manz 5:1da4d4050306 127 /*serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen);
manz 3:63aef644e6d2 128
manz 0:83acd45a2405 129 unsigned char i = 0;
manz 0:83acd45a2405 130 while(i<rxlen)
manz 0:83acd45a2405 131 {
manz 0:83acd45a2405 132 serial.printf("%d, ",val[i++]);
manz 0:83acd45a2405 133 }
manz 3:63aef644e6d2 134
manz 5:1da4d4050306 135 serial.printf("\r\n");*/
manz 3:63aef644e6d2 136 //determine mode of signal
manz 3:63aef644e6d2 137 if(rxlen == 1){
manz 5:1da4d4050306 138 // inputs blocked until shape finished
manz 3:63aef644e6d2 139 if (mode != 13){
manz 3:63aef644e6d2 140 mode = (int) val[0];
manz 5:1da4d4050306 141 if(mode != 4 && mode !=6){
manz 7:1bb3b5b66fe8 142 serial.printf("Mode 1-val: %i\r\n",mode);
manz 7:1bb3b5b66fe8 143 osSignalClear(bluetooth_id,0x1);
manz 5:1da4d4050306 144 which_thread = 1;
manz 5:1da4d4050306 145 }
manz 5:1da4d4050306 146 }
manz 0:83acd45a2405 147 }
manz 5:1da4d4050306 148 if (mode == 6){
manz 7:1bb3b5b66fe8 149 stroke = 0;
manz 5:1da4d4050306 150 serial.printf("Storing drawn coordinates\r\n");
manz 5:1da4d4050306 151 serial.printf("x_coord = [");
manz 5:1da4d4050306 152 for (int i = 0; i < size; i++) {
manz 5:1da4d4050306 153 serial.printf("%i ", path_x[i]);
manz 5:1da4d4050306 154 }
manz 5:1da4d4050306 155 serial.printf("];\n");
manz 5:1da4d4050306 156 serial.printf("y_coord = [");
manz 5:1da4d4050306 157 for (int i = 0; i < size; i++) {
manz 5:1da4d4050306 158 serial.printf("%i ", path_y[i]);
manz 5:1da4d4050306 159 }
manz 5:1da4d4050306 160 serial.printf("];\n");
manz 5:1da4d4050306 161 }
manz 5:1da4d4050306 162 // stroke of pen finished
manz 5:1da4d4050306 163 if (mode == 7){
manz 7:1bb3b5b66fe8 164 stroke = 1;
manz 5:1da4d4050306 165 serial.printf("Other threads can proceed \r\n");
manz 5:1da4d4050306 166 osSignalClear(bluetooth_id,0x1);
manz 5:1da4d4050306 167 which_thread = 1;
manz 5:1da4d4050306 168 }
manz 5:1da4d4050306 169 if(rxlen == 5){
manz 5:1da4d4050306 170 // in coordinates mode - accept negative coordinates
manz 7:1bb3b5b66fe8 171 if(mode != 4 && stroke == 0){
manz 3:63aef644e6d2 172 mode = 9;
manz 5:1da4d4050306 173 }
manz 5:1da4d4050306 174 // in draw mode - convert coordinates
manz 5:1da4d4050306 175 x = (int) val[0]*256 + val[1];
manz 5:1da4d4050306 176 y = (int) val[2]*256 + val[3];
manz 3:63aef644e6d2 177
manz 3:63aef644e6d2 178 // putting values into array
manz 5:1da4d4050306 179 //serial.printf("try to put coordinates \r\n");
manz 3:63aef644e6d2 180 one_slot.wait();
manz 3:63aef644e6d2 181 if (counter == size){
manz 5:1da4d4050306 182 serial.printf("Overwriting values \r\n");
manz 3:63aef644e6d2 183 counter = 0;
manz 3:63aef644e6d2 184 }
manz 3:63aef644e6d2 185 path_x[counter] = x;
manz 3:63aef644e6d2 186 path_y[counter] = y;
manz 5:1da4d4050306 187
manz 5:1da4d4050306 188 //check if pen needs to be up or down
manz 7:1bb3b5b66fe8 189 path_p[counter] = (int) val[4];
manz 7:1bb3b5b66fe8 190 serial.printf("Pen mode: %i \r\n",path_p[counter]);
manz 5:1da4d4050306 191
manz 3:63aef644e6d2 192 counter++;
manz 3:63aef644e6d2 193 one_slot.release();
manz 5:1da4d4050306 194 if(mode == 9){
manz 7:1bb3b5b66fe8 195 serial.printf("coordinate mode");
manz 7:1bb3b5b66fe8 196 osSignalClear(bluetooth_id,0x1);
manz 5:1da4d4050306 197 which_thread = 1;
manz 5:1da4d4050306 198 }
manz 5:1da4d4050306 199 //serial.printf("put coordinates \r\n");
manz 0:83acd45a2405 200 }
manz 0:83acd45a2405 201 }
manz 0:83acd45a2405 202
manz 0:83acd45a2405 203 if(serial.readable())
manz 0:83acd45a2405 204 {
manz 0:83acd45a2405 205 if(!count_on)
manz 0:83acd45a2405 206 {
manz 0:83acd45a2405 207 count_on = 1;
manz 0:83acd45a2405 208 }
manz 0:83acd45a2405 209 interval = 0;
manz 0:83acd45a2405 210 txbuf[txlen] = serial.getc();
manz 0:83acd45a2405 211 txlen++;
manz 0:83acd45a2405 212 }
manz 0:83acd45a2405 213
manz 0:83acd45a2405 214 if(count_on) // Count the interval after receiving a new char from terminate
manz 0:83acd45a2405 215 {
manz 0:83acd45a2405 216 interval++;
manz 0:83acd45a2405 217 }
manz 0:83acd45a2405 218
manz 0:83acd45a2405 219 if(interval == 10) // If there is no char available last the interval, send the received chars to central.
manz 0:83acd45a2405 220 {
manz 0:83acd45a2405 221 interval = 0;
manz 0:83acd45a2405 222 count_on = 0;
manz 5:1da4d4050306 223 //serial.printf("Received from terminal: %d bytes\r\n", txlen);
manz 0:83acd45a2405 224 txCharacteristic.setValue((const unsigned char *)txbuf, txlen);
manz 0:83acd45a2405 225 txlen = 0;
manz 0:83acd45a2405 226 }
manz 0:83acd45a2405 227 }
manz 5:1da4d4050306 228
manz 0:83acd45a2405 229 // central disconnected
manz 0:83acd45a2405 230 serial.printf("Disconnected from central\r\n");
manz 0:83acd45a2405 231 }
manz 0:83acd45a2405 232 }
manz 0:83acd45a2405 233 }
manz 0:83acd45a2405 234
manz 5:1da4d4050306 235
manz 3:63aef644e6d2 236 // incomplete
manz 3:63aef644e6d2 237 int draw_circle(double radius){
manz 3:63aef644e6d2 238 float currAngle = L.getAngle();
manz 3:63aef644e6d2 239 if(radius >= 5){
manz 3:63aef644e6d2 240 int outer = (int) radius*0.6;
manz 3:63aef644e6d2 241 int inner = (int) radius*0.2;
manz 3:63aef644e6d2 242 }
manz 3:63aef644e6d2 243 }
manz 3:63aef644e6d2 244
manz 0:83acd45a2405 245
manz 0:83acd45a2405 246 void move_thread(void const *args){
manz 5:1da4d4050306 247
manz 0:83acd45a2405 248 int angle;
manz 9:946d400b2f13 249 double length = 8;
manz 9:946d400b2f13 250 double width = 8;
manz 3:63aef644e6d2 251 int radius = 5;
manz 0:83acd45a2405 252
manz 0:83acd45a2405 253 Timer t;
manz 0:83acd45a2405 254 //initally put pen down
manz 7:1bb3b5b66fe8 255 servo_reset();
manz 0:83acd45a2405 256
manz 0:83acd45a2405 257 float currentAngle;
manz 0:83acd45a2405 258 float targetAngle;
manz 0:83acd45a2405 259 float startAngle;
manz 0:83acd45a2405 260 float diffAngle;
manz 0:83acd45a2405 261
manz 5:1da4d4050306 262 int steps;
manz 5:1da4d4050306 263
manz 0:83acd45a2405 264 int control;
manz 7:1bb3b5b66fe8 265 int restore = 0;
manz 3:63aef644e6d2 266 int shape_count = 0;
manz 5:1da4d4050306 267 int p_mode;
manz 5:1da4d4050306 268 int first = 0;
manz 3:63aef644e6d2 269
manz 9:946d400b2f13 270 double draw_corr = 2;
nibab 8:4d7b2dbdb694 271 double time_factor = 0.05;
manz 0:83acd45a2405 272
manz 0:83acd45a2405 273 double dot,a2,b2,c2;
manz 0:83acd45a2405 274 double x_tar,y_tar;
manz 7:1bb3b5b66fe8 275 double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr, x_taro_pr, y_taro_pr;
manz 0:83acd45a2405 276 double cosg,gamma,distance,wait_t;
manz 0:83acd45a2405 277 double x_taro = 0;
manz 0:83acd45a2405 278 double y_taro = 0;
manz 0:83acd45a2405 279 serial.printf("Started move thread\r\n");
manz 0:83acd45a2405 280 int newval = 0;
manz 9:946d400b2f13 281
manz 9:946d400b2f13 282 //values for encoding
manz 9:946d400b2f13 283 int counter_l = 0;
manz 9:946d400b2f13 284 int on_l;
manz 9:946d400b2f13 285 int counter_r = 0;
manz 9:946d400b2f13 286 int on_r;
manz 9:946d400b2f13 287 double sum_r = 0;
manz 9:946d400b2f13 288 double sum_l = 0;
manz 9:946d400b2f13 289 double value_r;
manz 9:946d400b2f13 290 double value_l;
manz 5:1da4d4050306 291
manz 0:83acd45a2405 292 while(1){
manz 3:63aef644e6d2 293 // check what mode is present
manz 5:1da4d4050306 294 serial.printf("Cylce \r\n");
manz 7:1bb3b5b66fe8 295 serial.printf("mode: %i \r\n",mode);
manz 7:1bb3b5b66fe8 296
manz 5:1da4d4050306 297 Thread::signal_wait(0x1);
manz 7:1bb3b5b66fe8 298
manz 5:1da4d4050306 299 if(mode == -1){
manz 5:1da4d4050306 300 serial.printf("here ends");
manz 5:1da4d4050306 301 osSignalClear(mover_id,0x1);
manz 5:1da4d4050306 302 which_thread = 0;
manz 5:1da4d4050306 303 }
manz 3:63aef644e6d2 304 //rectangle/square
manz 5:1da4d4050306 305 else if(mode == 0){
manz 3:63aef644e6d2 306 serial.printf("Draw rectangle \r\n");
manz 3:63aef644e6d2 307 //save old values and set initial
manz 3:63aef644e6d2 308 x_pen_pr = x_pen;
manz 3:63aef644e6d2 309 y_pen_pr = y_pen;
manz 3:63aef644e6d2 310 x_cent_pr = x_cent;
manz 7:1bb3b5b66fe8 311 y_cent_pr = y_cent;
manz 7:1bb3b5b66fe8 312 x_taro_pr = x_taro;
manz 7:1bb3b5b66fe8 313 y_taro_pr = y_taro;
manz 3:63aef644e6d2 314 x_pen = 0;
manz 3:63aef644e6d2 315 y_pen = 0;
manz 3:63aef644e6d2 316 x_cent = 0;
manz 3:63aef644e6d2 317 y_cent = -1;
manz 5:1da4d4050306 318 x_taro = 0;
manz 5:1da4d4050306 319 y_taro = 0;
manz 3:63aef644e6d2 320
manz 3:63aef644e6d2 321 //set values
manz 7:1bb3b5b66fe8 322 shape_x[4] = 0;
manz 7:1bb3b5b66fe8 323 shape_y[4] = length;
manz 7:1bb3b5b66fe8 324 shape_x[3] = width;
manz 3:63aef644e6d2 325 shape_y[3] = length;
manz 3:63aef644e6d2 326 shape_x[2] = width;
manz 7:1bb3b5b66fe8 327 shape_y[2] = 0;
manz 7:1bb3b5b66fe8 328 shape_x[1] = 0;
manz 3:63aef644e6d2 329 shape_y[1] = 0;
manz 3:63aef644e6d2 330 shape_x[0] = 0;
manz 7:1bb3b5b66fe8 331 shape_y[0] = 0.1;
manz 7:1bb3b5b66fe8 332 shape_count = 5;
manz 3:63aef644e6d2 333
manz 3:63aef644e6d2 334 mode = 13;
manz 3:63aef644e6d2 335 }
manz 3:63aef644e6d2 336 else if(mode == 1){
manz 3:63aef644e6d2 337 serial.printf("Draw circle \r\n");
manz 3:63aef644e6d2 338 //call circle function
manz 3:63aef644e6d2 339 control = draw_circle(radius);
manz 3:63aef644e6d2 340
manz 3:63aef644e6d2 341 mode = -1;
manz 3:63aef644e6d2 342 }
manz 3:63aef644e6d2 343 else if(mode == 2){
manz 3:63aef644e6d2 344 serial.printf("Draw triangle \r\n");
manz 3:63aef644e6d2 345 //save old values and set initial
manz 3:63aef644e6d2 346 x_pen_pr = x_pen;
manz 3:63aef644e6d2 347 y_pen_pr = y_pen;
manz 3:63aef644e6d2 348 x_cent_pr = x_cent;
manz 7:1bb3b5b66fe8 349 y_cent_pr = y_cent;
manz 7:1bb3b5b66fe8 350 x_taro_pr = x_taro;
manz 7:1bb3b5b66fe8 351 y_taro_pr = y_taro;
manz 3:63aef644e6d2 352 x_pen = 0;
manz 3:63aef644e6d2 353 y_pen = 0;
manz 3:63aef644e6d2 354 x_cent = 0;
manz 3:63aef644e6d2 355 y_cent = -1;
manz 5:1da4d4050306 356 x_taro = 0;
manz 5:1da4d4050306 357 y_taro = 0;
manz 3:63aef644e6d2 358
manz 3:63aef644e6d2 359 //set values
manz 7:1bb3b5b66fe8 360 shape_x[3] = 0;
manz 7:1bb3b5b66fe8 361 shape_y[3] = length;
manz 7:1bb3b5b66fe8 362 shape_x[2] = width;
manz 7:1bb3b5b66fe8 363 shape_y[2] = 0;
manz 7:1bb3b5b66fe8 364 shape_x[1] = 0;
manz 3:63aef644e6d2 365 shape_y[1] = 0;
manz 3:63aef644e6d2 366 shape_x[0] = 0;
manz 7:1bb3b5b66fe8 367 shape_y[0] = 0.1;
manz 3:63aef644e6d2 368
manz 7:1bb3b5b66fe8 369 shape_count = 4;
manz 3:63aef644e6d2 370 mode = 13;
manz 3:63aef644e6d2 371 }
manz 3:63aef644e6d2 372 else if(mode == 3){
manz 3:63aef644e6d2 373 serial.printf("Reset \r\n");
manz 3:63aef644e6d2 374 //set initial
manz 3:63aef644e6d2 375 x_pen = 0;
manz 3:63aef644e6d2 376 y_pen = 0;
manz 3:63aef644e6d2 377 x_cent = 0;
manz 3:63aef644e6d2 378 y_cent = -1;
manz 7:1bb3b5b66fe8 379 x_taro = 0;
manz 7:1bb3b5b66fe8 380 y_taro = 0;
manz 7:1bb3b5b66fe8 381
manz 7:1bb3b5b66fe8 382 one_slot.wait();
manz 7:1bb3b5b66fe8 383 counter = 0;
manz 7:1bb3b5b66fe8 384 start = 0;
manz 7:1bb3b5b66fe8 385 one_slot.release();
manz 3:63aef644e6d2 386
manz 3:63aef644e6d2 387 mode = -1;
manz 3:63aef644e6d2 388 }
manz 5:1da4d4050306 389 else if (mode == 7){
manz 5:1da4d4050306 390 if (first == 0){
manz 5:1da4d4050306 391 serial.printf("Draw freely \r\n");
manz 5:1da4d4050306 392 draw_corr = 1;
manz 5:1da4d4050306 393 }
manz 5:1da4d4050306 394 first = 1;
manz 3:63aef644e6d2 395 }
manz 3:63aef644e6d2 396 else if (mode == 9){
manz 3:63aef644e6d2 397 serial.printf("Draw coordinates \r\n");
manz 9:946d400b2f13 398 draw_corr = 2;
manz 3:63aef644e6d2 399 mode = -1;
manz 3:63aef644e6d2 400 }
manz 3:63aef644e6d2 401
manz 3:63aef644e6d2 402 // next coordinate is free drawing or coordinates
manz 3:63aef644e6d2 403 if(shape_count == 0){
manz 3:63aef644e6d2 404 one_slot.wait();
manz 3:63aef644e6d2 405 if(counter != start){
manz 3:63aef644e6d2 406 if (start == size){
manz 3:63aef644e6d2 407 start = 0;
manz 3:63aef644e6d2 408 }
manz 5:1da4d4050306 409 x_tar = (double) path_x[start]*draw_corr;
manz 5:1da4d4050306 410 y_tar = (double) path_y[start]*draw_corr;
manz 5:1da4d4050306 411 p_mode = path_p[start];
manz 3:63aef644e6d2 412 start++;
manz 7:1bb3b5b66fe8 413 if(start == counter){
manz 7:1bb3b5b66fe8 414 mode = -1;
manz 7:1bb3b5b66fe8 415 }
manz 3:63aef644e6d2 416 newval = 1;
manz 0:83acd45a2405 417 }
manz 3:63aef644e6d2 418 one_slot.release();
manz 0:83acd45a2405 419 }
manz 3:63aef644e6d2 420 // next coordinate is shape
manz 3:63aef644e6d2 421 else{
manz 3:63aef644e6d2 422 shape_count = shape_count - 1;
manz 7:1bb3b5b66fe8 423 x_tar = shape_x[shape_count];
manz 7:1bb3b5b66fe8 424 y_tar = shape_y[shape_count];
manz 5:1da4d4050306 425 p_mode = 1;
manz 3:63aef644e6d2 426 newval = 1;
manz 5:1da4d4050306 427
manz 5:1da4d4050306 428 //last move -> unblock input
manz 5:1da4d4050306 429 if(shape_count == 0){
manz 7:1bb3b5b66fe8 430 restore = 1;
manz 5:1da4d4050306 431 mode = -1;
manz 5:1da4d4050306 432 }
manz 3:63aef644e6d2 433 }
manz 0:83acd45a2405 434 if(newval == 1){
manz 0:83acd45a2405 435 serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar);
manz 0:83acd45a2405 436 newval = 0;
manz 7:1bb3b5b66fe8 437 // same position -> do nothing
manz 7:1bb3b5b66fe8 438 if(x_tar == x_pen && y_tar == y_pen){
manz 7:1bb3b5b66fe8 439
manz 5:1da4d4050306 440 }
manz 7:1bb3b5b66fe8 441 else{
manz 7:1bb3b5b66fe8 442 //compute angle and turn direction
manz 7:1bb3b5b66fe8 443 a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent);
manz 7:1bb3b5b66fe8 444 b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen);
manz 7:1bb3b5b66fe8 445 c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent);
manz 7:1bb3b5b66fe8 446 cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2));
manz 7:1bb3b5b66fe8 447 gamma = acos(cosg)/3.14159*180;
manz 7:1bb3b5b66fe8 448
manz 7:1bb3b5b66fe8 449 dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent);
manz 7:1bb3b5b66fe8 450
manz 7:1bb3b5b66fe8 451 //serial.printf("Angle: %f \r\n",gamma);
manz 7:1bb3b5b66fe8 452 angle = ceil(180 - gamma);
manz 7:1bb3b5b66fe8 453 serial.printf("Turning angle: %i \r\n",angle);
manz 7:1bb3b5b66fe8 454
manz 7:1bb3b5b66fe8 455 //put pen down
manz 7:1bb3b5b66fe8 456 if (p_mode == 1){
manz 7:1bb3b5b66fe8 457 serial.printf("-- Pen Down\r\n");
manz 9:946d400b2f13 458 L.servo(30);
manz 0:83acd45a2405 459 }
manz 7:1bb3b5b66fe8 460 //put pen up
manz 7:1bb3b5b66fe8 461 else if (p_mode == 0){
manz 9:946d400b2f13 462 L.servo(45);
manz 7:1bb3b5b66fe8 463 serial.printf("-- Pen Up\r\n");
manz 7:1bb3b5b66fe8 464 }
manz 7:1bb3b5b66fe8 465
manz 7:1bb3b5b66fe8 466 currentAngle = L.getAngle();
manz 7:1bb3b5b66fe8 467 if(dot > 0){
manz 7:1bb3b5b66fe8 468 //serial.printf("Turn right \r\n");
manz 7:1bb3b5b66fe8 469 targetAngle = fmod(currentAngle+angle,360);
manz 7:1bb3b5b66fe8 470
manz 7:1bb3b5b66fe8 471 servo_right();
manz 7:1bb3b5b66fe8 472 while(fmod(abs(L.getAngle() - targetAngle),360)> 3);
manz 7:1bb3b5b66fe8 473 servo_stop();
manz 0:83acd45a2405 474 }
manz 7:1bb3b5b66fe8 475 else if(dot < 0){
manz 7:1bb3b5b66fe8 476 //serial.printf("Turn left \r\n");
manz 7:1bb3b5b66fe8 477 targetAngle = fmod(currentAngle-angle,360);
manz 7:1bb3b5b66fe8 478 servo_left();
manz 7:1bb3b5b66fe8 479 while(fmod(abs(L.getAngle() - targetAngle),360)> 3);
manz 7:1bb3b5b66fe8 480 servo_stop();
manz 7:1bb3b5b66fe8 481 }
manz 7:1bb3b5b66fe8 482 //compute length of path til target
manz 7:1bb3b5b66fe8 483 distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen));
manz 7:1bb3b5b66fe8 484
manz 7:1bb3b5b66fe8 485 //forward SCRIBE til target
manz 7:1bb3b5b66fe8 486 serial.printf("Distance to be taken: %f\r\n",distance);
manz 7:1bb3b5b66fe8 487
manz 9:946d400b2f13 488 int counter_m = (int) rint(distance);
manz 9:946d400b2f13 489
manz 9:946d400b2f13 490 // find position of wheels
manz 9:946d400b2f13 491 for (int i = 0; i < 100; i++){
manz 9:946d400b2f13 492 sum_r += ain1.read();
manz 9:946d400b2f13 493 sum_l += ain2.read();
manz 9:946d400b2f13 494 }
manz 9:946d400b2f13 495 value_r = sum_r/100;
manz 9:946d400b2f13 496 value_l = sum_l/100;
manz 9:946d400b2f13 497
manz 9:946d400b2f13 498 if(value_r < 0.93){
manz 9:946d400b2f13 499 on_r = 0;
manz 9:946d400b2f13 500 }
manz 9:946d400b2f13 501 else{
manz 9:946d400b2f13 502 on_r = 1;
manz 9:946d400b2f13 503 }
manz 9:946d400b2f13 504 if(value_l < 0.93){
manz 9:946d400b2f13 505 on_l = 0;
manz 9:946d400b2f13 506 }
manz 9:946d400b2f13 507 else{
manz 9:946d400b2f13 508 on_l = 1;
manz 9:946d400b2f13 509 }
manz 9:946d400b2f13 510 serial.printf("Initially: left: %d, right: %d\r\n",on_l,on_r);
manz 7:1bb3b5b66fe8 511 servo_f();
manz 9:946d400b2f13 512 //start decrement counter
manz 9:946d400b2f13 513 counter_r = counter_m;
manz 9:946d400b2f13 514 counter_l = counter_m;
manz 9:946d400b2f13 515 while (counter_m > 0){
manz 9:946d400b2f13 516 sum_r = 0;
manz 9:946d400b2f13 517 sum_l = 0;
manz 9:946d400b2f13 518 for (int i = 0; i < 100; i++){
manz 9:946d400b2f13 519 sum_r += ain1.read();
manz 9:946d400b2f13 520 sum_l += ain2.read();
manz 9:946d400b2f13 521 }
manz 9:946d400b2f13 522 value_r = sum_r/100;
manz 9:946d400b2f13 523 value_l = sum_l/100;
manz 9:946d400b2f13 524 //printf("Value: %f\r\n",value);
manz 9:946d400b2f13 525 if(value_r < 0.93 && on_r == 1){
manz 9:946d400b2f13 526 printf("Value right: %f\r\n",value_r);
manz 9:946d400b2f13 527 on_r = 0;
manz 9:946d400b2f13 528 }
manz 9:946d400b2f13 529 else if (value_r > 0.95 && on_r == 0){
manz 9:946d400b2f13 530 printf("Value right: %f\r\n",value_r);
manz 9:946d400b2f13 531 on_r = 1;
manz 9:946d400b2f13 532 counter_r--;
manz 9:946d400b2f13 533 printf("*** Right Counter is: %d\r\n",counter_r);
manz 9:946d400b2f13 534 }
manz 9:946d400b2f13 535 if(value_l < 0.93 && on_l == 1){
manz 9:946d400b2f13 536 printf("Value left: %f\r\n",value_l);
manz 9:946d400b2f13 537 on_l = 0;
manz 9:946d400b2f13 538 }
manz 9:946d400b2f13 539 else if (value_l > 0.95 && on_l == 0){
manz 9:946d400b2f13 540 printf("Value left: %f\r\n",value_l);
manz 9:946d400b2f13 541 on_l = 1;
manz 9:946d400b2f13 542 counter_l--;
manz 9:946d400b2f13 543 printf("*** Left Counter is: %d\r\n",counter_l);
manz 9:946d400b2f13 544 }
manz 9:946d400b2f13 545
manz 9:946d400b2f13 546 if(counter_l >= counter_r){
manz 9:946d400b2f13 547 counter_m = counter_r;
manz 9:946d400b2f13 548 }
manz 9:946d400b2f13 549 else{
manz 9:946d400b2f13 550 counter_m = counter_l;
manz 9:946d400b2f13 551 }
manz 9:946d400b2f13 552 printf("*** Distance to take is: %d\r\n",counter_m);
manz 9:946d400b2f13 553 }
manz 9:946d400b2f13 554 //servo_f();
manz 9:946d400b2f13 555 //wait(distance*time_factor);
manz 7:1bb3b5b66fe8 556 servo_stop();
manz 7:1bb3b5b66fe8 557 //serial.printf("Reached destination \r\n");
manz 7:1bb3b5b66fe8 558 //update pen and center when at target
manz 7:1bb3b5b66fe8 559 if(restore == 1){
manz 7:1bb3b5b66fe8 560 serial.printf("Restore previous positions \r\n");
manz 7:1bb3b5b66fe8 561 x_pen = x_pen_pr;
manz 7:1bb3b5b66fe8 562 y_pen = y_pen_pr;
manz 7:1bb3b5b66fe8 563 x_cent = x_cent_pr;
manz 7:1bb3b5b66fe8 564 y_cent = y_cent_pr;
manz 7:1bb3b5b66fe8 565 x_taro = x_taro_pr;
manz 7:1bb3b5b66fe8 566 y_taro = y_taro_pr;
manz 7:1bb3b5b66fe8 567 restore = 0;
manz 7:1bb3b5b66fe8 568 }
manz 7:1bb3b5b66fe8 569 else{
manz 7:1bb3b5b66fe8 570 x_pen = x_tar;
manz 7:1bb3b5b66fe8 571 y_pen = y_tar;
manz 7:1bb3b5b66fe8 572 x_cent = x_taro;
manz 7:1bb3b5b66fe8 573 y_cent = y_taro;
manz 7:1bb3b5b66fe8 574 x_taro = x_tar;
manz 7:1bb3b5b66fe8 575 y_taro = y_tar;
manz 7:1bb3b5b66fe8 576 }
manz 7:1bb3b5b66fe8 577 }
manz 0:83acd45a2405 578 serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen);
manz 7:1bb3b5b66fe8 579 serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent);
manz 5:1da4d4050306 580 }
manz 0:83acd45a2405 581 }
manz 0:83acd45a2405 582 }
manz 0:83acd45a2405 583
manz 5:1da4d4050306 584
manz 5:1da4d4050306 585
manz 0:83acd45a2405 586 int main()
manz 7:1bb3b5b66fe8 587 {
manz 7:1bb3b5b66fe8 588 L.reset();
manz 7:1bb3b5b66fe8 589 //L.servo(90);
manz 7:1bb3b5b66fe8 590 wait(1);
manz 7:1bb3b5b66fe8 591 serial.printf("Starting Dead-Reckoning\r\n");
manz 7:1bb3b5b66fe8 592 //deadreckoning();
manz 7:1bb3b5b66fe8 593 serial.printf("Starting the threads\r\n");
manz 0:83acd45a2405 594
manz 0:83acd45a2405 595 Thread bluetooth(bluetooth_thread);
manz 0:83acd45a2405 596 Thread move(move_thread);
manz 5:1da4d4050306 597 mover_id = move.gettid();
manz 5:1da4d4050306 598 bluetooth_id = bluetooth.gettid();
manz 0:83acd45a2405 599
manz 5:1da4d4050306 600 while(1){
manz 7:1bb3b5b66fe8 601 Thread::wait(3);
manz 5:1da4d4050306 602 if(which_thread == 0) bluetooth.signal_set(0x1);
manz 5:1da4d4050306 603 else move.signal_set(0x1);
manz 5:1da4d4050306 604 }
manz 0:83acd45a2405 605 }