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@12:01250ea24795, 2016-05-05 (annotated)

Committer:
manz
Date:
Thu May 05 23:56:48 2016 +0000
Revision:
12:01250ea24795
Parent:
9:946d400b2f13
Child:
13:d49cb8b52a1e
correction 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 12:01250ea24795 256 servo_stop();
manz 0:83acd45a2405 257
manz 0:83acd45a2405 258 float currentAngle;
manz 0:83acd45a2405 259 float targetAngle;
manz 0:83acd45a2405 260 float startAngle;
manz 0:83acd45a2405 261 float diffAngle;
manz 0:83acd45a2405 262
manz 5:1da4d4050306 263 int steps;
manz 5:1da4d4050306 264
manz 0:83acd45a2405 265 int control;
manz 7:1bb3b5b66fe8 266 int restore = 0;
manz 3:63aef644e6d2 267 int shape_count = 0;
manz 5:1da4d4050306 268 int p_mode;
manz 5:1da4d4050306 269 int first = 0;
manz 3:63aef644e6d2 270
manz 9:946d400b2f13 271 double draw_corr = 2;
nibab 8:4d7b2dbdb694 272 double time_factor = 0.05;
manz 0:83acd45a2405 273
manz 0:83acd45a2405 274 double dot,a2,b2,c2;
manz 0:83acd45a2405 275 double x_tar,y_tar;
manz 7:1bb3b5b66fe8 276 double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr, x_taro_pr, y_taro_pr;
manz 0:83acd45a2405 277 double cosg,gamma,distance,wait_t;
manz 0:83acd45a2405 278 double x_taro = 0;
manz 0:83acd45a2405 279 double y_taro = 0;
manz 0:83acd45a2405 280 serial.printf("Started move thread\r\n");
manz 0:83acd45a2405 281 int newval = 0;
manz 9:946d400b2f13 282
manz 9:946d400b2f13 283 //values for encoding
manz 9:946d400b2f13 284 int counter_l = 0;
manz 9:946d400b2f13 285 int on_l;
manz 9:946d400b2f13 286 int counter_r = 0;
manz 9:946d400b2f13 287 int on_r;
manz 9:946d400b2f13 288 double sum_r = 0;
manz 9:946d400b2f13 289 double sum_l = 0;
manz 9:946d400b2f13 290 double value_r;
manz 9:946d400b2f13 291 double value_l;
manz 12:01250ea24795 292 float angle_init;
manz 12:01250ea24795 293
manz 12:01250ea24795 294 float testAngle = L.getAngle();
manz 5:1da4d4050306 295
manz 0:83acd45a2405 296 while(1){
manz 3:63aef644e6d2 297 // check what mode is present
manz 5:1da4d4050306 298 serial.printf("Cylce \r\n");
manz 7:1bb3b5b66fe8 299 serial.printf("mode: %i \r\n",mode);
manz 7:1bb3b5b66fe8 300
manz 5:1da4d4050306 301 Thread::signal_wait(0x1);
manz 7:1bb3b5b66fe8 302
manz 5:1da4d4050306 303 if(mode == -1){
manz 5:1da4d4050306 304 serial.printf("here ends");
manz 5:1da4d4050306 305 osSignalClear(mover_id,0x1);
manz 5:1da4d4050306 306 which_thread = 0;
manz 5:1da4d4050306 307 }
manz 3:63aef644e6d2 308 //rectangle/square
manz 5:1da4d4050306 309 else if(mode == 0){
manz 3:63aef644e6d2 310 serial.printf("Draw rectangle \r\n");
manz 3:63aef644e6d2 311 //save old values and set initial
manz 3:63aef644e6d2 312 x_pen_pr = x_pen;
manz 3:63aef644e6d2 313 y_pen_pr = y_pen;
manz 3:63aef644e6d2 314 x_cent_pr = x_cent;
manz 7:1bb3b5b66fe8 315 y_cent_pr = y_cent;
manz 7:1bb3b5b66fe8 316 x_taro_pr = x_taro;
manz 7:1bb3b5b66fe8 317 y_taro_pr = y_taro;
manz 3:63aef644e6d2 318 x_pen = 0;
manz 3:63aef644e6d2 319 y_pen = 0;
manz 3:63aef644e6d2 320 x_cent = 0;
manz 3:63aef644e6d2 321 y_cent = -1;
manz 5:1da4d4050306 322 x_taro = 0;
manz 5:1da4d4050306 323 y_taro = 0;
manz 3:63aef644e6d2 324
manz 3:63aef644e6d2 325 //set values
manz 7:1bb3b5b66fe8 326 shape_x[4] = 0;
manz 7:1bb3b5b66fe8 327 shape_y[4] = length;
manz 7:1bb3b5b66fe8 328 shape_x[3] = width;
manz 3:63aef644e6d2 329 shape_y[3] = length;
manz 3:63aef644e6d2 330 shape_x[2] = width;
manz 7:1bb3b5b66fe8 331 shape_y[2] = 0;
manz 7:1bb3b5b66fe8 332 shape_x[1] = 0;
manz 3:63aef644e6d2 333 shape_y[1] = 0;
manz 3:63aef644e6d2 334 shape_x[0] = 0;
manz 7:1bb3b5b66fe8 335 shape_y[0] = 0.1;
manz 7:1bb3b5b66fe8 336 shape_count = 5;
manz 3:63aef644e6d2 337
manz 3:63aef644e6d2 338 mode = 13;
manz 3:63aef644e6d2 339 }
manz 3:63aef644e6d2 340 else if(mode == 1){
manz 3:63aef644e6d2 341 serial.printf("Draw circle \r\n");
manz 3:63aef644e6d2 342 //call circle function
manz 3:63aef644e6d2 343 control = draw_circle(radius);
manz 3:63aef644e6d2 344
manz 3:63aef644e6d2 345 mode = -1;
manz 3:63aef644e6d2 346 }
manz 3:63aef644e6d2 347 else if(mode == 2){
manz 3:63aef644e6d2 348 serial.printf("Draw triangle \r\n");
manz 3:63aef644e6d2 349 //save old values and set initial
manz 3:63aef644e6d2 350 x_pen_pr = x_pen;
manz 3:63aef644e6d2 351 y_pen_pr = y_pen;
manz 3:63aef644e6d2 352 x_cent_pr = x_cent;
manz 7:1bb3b5b66fe8 353 y_cent_pr = y_cent;
manz 7:1bb3b5b66fe8 354 x_taro_pr = x_taro;
manz 7:1bb3b5b66fe8 355 y_taro_pr = y_taro;
manz 3:63aef644e6d2 356 x_pen = 0;
manz 3:63aef644e6d2 357 y_pen = 0;
manz 3:63aef644e6d2 358 x_cent = 0;
manz 3:63aef644e6d2 359 y_cent = -1;
manz 5:1da4d4050306 360 x_taro = 0;
manz 5:1da4d4050306 361 y_taro = 0;
manz 3:63aef644e6d2 362
manz 3:63aef644e6d2 363 //set values
manz 7:1bb3b5b66fe8 364 shape_x[3] = 0;
manz 7:1bb3b5b66fe8 365 shape_y[3] = length;
manz 7:1bb3b5b66fe8 366 shape_x[2] = width;
manz 7:1bb3b5b66fe8 367 shape_y[2] = 0;
manz 7:1bb3b5b66fe8 368 shape_x[1] = 0;
manz 3:63aef644e6d2 369 shape_y[1] = 0;
manz 3:63aef644e6d2 370 shape_x[0] = 0;
manz 7:1bb3b5b66fe8 371 shape_y[0] = 0.1;
manz 3:63aef644e6d2 372
manz 7:1bb3b5b66fe8 373 shape_count = 4;
manz 3:63aef644e6d2 374 mode = 13;
manz 3:63aef644e6d2 375 }
manz 3:63aef644e6d2 376 else if(mode == 3){
manz 3:63aef644e6d2 377 serial.printf("Reset \r\n");
manz 3:63aef644e6d2 378 //set initial
manz 3:63aef644e6d2 379 x_pen = 0;
manz 3:63aef644e6d2 380 y_pen = 0;
manz 3:63aef644e6d2 381 x_cent = 0;
manz 3:63aef644e6d2 382 y_cent = -1;
manz 7:1bb3b5b66fe8 383 x_taro = 0;
manz 7:1bb3b5b66fe8 384 y_taro = 0;
manz 7:1bb3b5b66fe8 385
manz 7:1bb3b5b66fe8 386 one_slot.wait();
manz 7:1bb3b5b66fe8 387 counter = 0;
manz 7:1bb3b5b66fe8 388 start = 0;
manz 7:1bb3b5b66fe8 389 one_slot.release();
manz 3:63aef644e6d2 390
manz 3:63aef644e6d2 391 mode = -1;
manz 3:63aef644e6d2 392 }
manz 5:1da4d4050306 393 else if (mode == 7){
manz 5:1da4d4050306 394 if (first == 0){
manz 5:1da4d4050306 395 serial.printf("Draw freely \r\n");
manz 5:1da4d4050306 396 draw_corr = 1;
manz 5:1da4d4050306 397 }
manz 5:1da4d4050306 398 first = 1;
manz 3:63aef644e6d2 399 }
manz 3:63aef644e6d2 400 else if (mode == 9){
manz 3:63aef644e6d2 401 serial.printf("Draw coordinates \r\n");
manz 12:01250ea24795 402 draw_corr = 1;
manz 3:63aef644e6d2 403 mode = -1;
manz 3:63aef644e6d2 404 }
manz 3:63aef644e6d2 405
manz 3:63aef644e6d2 406 // next coordinate is free drawing or coordinates
manz 3:63aef644e6d2 407 if(shape_count == 0){
manz 3:63aef644e6d2 408 one_slot.wait();
manz 3:63aef644e6d2 409 if(counter != start){
manz 3:63aef644e6d2 410 if (start == size){
manz 3:63aef644e6d2 411 start = 0;
manz 3:63aef644e6d2 412 }
manz 5:1da4d4050306 413 x_tar = (double) path_x[start]*draw_corr;
manz 5:1da4d4050306 414 y_tar = (double) path_y[start]*draw_corr;
manz 5:1da4d4050306 415 p_mode = path_p[start];
manz 3:63aef644e6d2 416 start++;
manz 7:1bb3b5b66fe8 417 if(start == counter){
manz 7:1bb3b5b66fe8 418 mode = -1;
manz 7:1bb3b5b66fe8 419 }
manz 3:63aef644e6d2 420 newval = 1;
manz 0:83acd45a2405 421 }
manz 3:63aef644e6d2 422 one_slot.release();
manz 0:83acd45a2405 423 }
manz 3:63aef644e6d2 424 // next coordinate is shape
manz 3:63aef644e6d2 425 else{
manz 3:63aef644e6d2 426 shape_count = shape_count - 1;
manz 7:1bb3b5b66fe8 427 x_tar = shape_x[shape_count];
manz 7:1bb3b5b66fe8 428 y_tar = shape_y[shape_count];
manz 5:1da4d4050306 429 p_mode = 1;
manz 3:63aef644e6d2 430 newval = 1;
manz 5:1da4d4050306 431
manz 5:1da4d4050306 432 //last move -> unblock input
manz 5:1da4d4050306 433 if(shape_count == 0){
manz 7:1bb3b5b66fe8 434 restore = 1;
manz 5:1da4d4050306 435 mode = -1;
manz 5:1da4d4050306 436 }
manz 3:63aef644e6d2 437 }
manz 0:83acd45a2405 438 if(newval == 1){
manz 0:83acd45a2405 439 serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar);
manz 0:83acd45a2405 440 newval = 0;
manz 7:1bb3b5b66fe8 441 // same position -> do nothing
manz 7:1bb3b5b66fe8 442 if(x_tar == x_pen && y_tar == y_pen){
manz 7:1bb3b5b66fe8 443
manz 5:1da4d4050306 444 }
manz 7:1bb3b5b66fe8 445 else{
manz 7:1bb3b5b66fe8 446 //compute angle and turn direction
manz 7:1bb3b5b66fe8 447 a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent);
manz 7:1bb3b5b66fe8 448 b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen);
manz 7:1bb3b5b66fe8 449 c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent);
manz 7:1bb3b5b66fe8 450 cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2));
manz 7:1bb3b5b66fe8 451 gamma = acos(cosg)/3.14159*180;
manz 7:1bb3b5b66fe8 452
manz 7:1bb3b5b66fe8 453 dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent);
manz 7:1bb3b5b66fe8 454
manz 7:1bb3b5b66fe8 455 //serial.printf("Angle: %f \r\n",gamma);
manz 7:1bb3b5b66fe8 456 angle = ceil(180 - gamma);
manz 7:1bb3b5b66fe8 457 serial.printf("Turning angle: %i \r\n",angle);
manz 7:1bb3b5b66fe8 458
manz 7:1bb3b5b66fe8 459 //put pen down
manz 7:1bb3b5b66fe8 460 if (p_mode == 1){
manz 7:1bb3b5b66fe8 461 serial.printf("-- Pen Down\r\n");
manz 9:946d400b2f13 462 L.servo(30);
manz 0:83acd45a2405 463 }
manz 7:1bb3b5b66fe8 464 //put pen up
manz 7:1bb3b5b66fe8 465 else if (p_mode == 0){
manz 9:946d400b2f13 466 L.servo(45);
manz 7:1bb3b5b66fe8 467 serial.printf("-- Pen Up\r\n");
manz 7:1bb3b5b66fe8 468 }
manz 7:1bb3b5b66fe8 469
manz 7:1bb3b5b66fe8 470 currentAngle = L.getAngle();
manz 7:1bb3b5b66fe8 471 if(dot > 0){
manz 7:1bb3b5b66fe8 472 //serial.printf("Turn right \r\n");
manz 7:1bb3b5b66fe8 473 targetAngle = fmod(currentAngle+angle,360);
manz 7:1bb3b5b66fe8 474
manz 7:1bb3b5b66fe8 475 servo_right();
manz 12:01250ea24795 476 while(fmod(abs(L.getAngle() - targetAngle),360)> 5);
manz 7:1bb3b5b66fe8 477 servo_stop();
manz 0:83acd45a2405 478 }
manz 7:1bb3b5b66fe8 479 else if(dot < 0){
manz 7:1bb3b5b66fe8 480 //serial.printf("Turn left \r\n");
manz 7:1bb3b5b66fe8 481 targetAngle = fmod(currentAngle-angle,360);
manz 7:1bb3b5b66fe8 482 servo_left();
manz 12:01250ea24795 483 while(fmod(abs(L.getAngle() - targetAngle),360)> 5);
manz 7:1bb3b5b66fe8 484 servo_stop();
manz 7:1bb3b5b66fe8 485 }
manz 7:1bb3b5b66fe8 486 //compute length of path til target
manz 7:1bb3b5b66fe8 487 distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen));
manz 7:1bb3b5b66fe8 488
manz 7:1bb3b5b66fe8 489 //forward SCRIBE til target
manz 7:1bb3b5b66fe8 490 serial.printf("Distance to be taken: %f\r\n",distance);
manz 7:1bb3b5b66fe8 491
manz 9:946d400b2f13 492 int counter_m = (int) rint(distance);
manz 9:946d400b2f13 493
manz 9:946d400b2f13 494 // find position of wheels
manz 9:946d400b2f13 495 for (int i = 0; i < 100; i++){
manz 9:946d400b2f13 496 sum_r += ain1.read();
manz 9:946d400b2f13 497 sum_l += ain2.read();
manz 9:946d400b2f13 498 }
manz 9:946d400b2f13 499 value_r = sum_r/100;
manz 9:946d400b2f13 500 value_l = sum_l/100;
manz 9:946d400b2f13 501
manz 9:946d400b2f13 502 if(value_r < 0.93){
manz 9:946d400b2f13 503 on_r = 0;
manz 9:946d400b2f13 504 }
manz 9:946d400b2f13 505 else{
manz 9:946d400b2f13 506 on_r = 1;
manz 9:946d400b2f13 507 }
manz 9:946d400b2f13 508 if(value_l < 0.93){
manz 9:946d400b2f13 509 on_l = 0;
manz 9:946d400b2f13 510 }
manz 9:946d400b2f13 511 else{
manz 9:946d400b2f13 512 on_l = 1;
manz 9:946d400b2f13 513 }
manz 9:946d400b2f13 514 serial.printf("Initially: left: %d, right: %d\r\n",on_l,on_r);
manz 12:01250ea24795 515 angle_init = L.getAngle();
manz 7:1bb3b5b66fe8 516 servo_f();
manz 12:01250ea24795 517 serial.printf("Initial Angle: %f\r\n",angle_init);
manz 9:946d400b2f13 518 //start decrement counter
manz 9:946d400b2f13 519 counter_r = counter_m;
manz 9:946d400b2f13 520 counter_l = counter_m;
manz 9:946d400b2f13 521 while (counter_m > 0){
manz 9:946d400b2f13 522 sum_r = 0;
manz 9:946d400b2f13 523 sum_l = 0;
manz 9:946d400b2f13 524 for (int i = 0; i < 100; i++){
manz 9:946d400b2f13 525 sum_r += ain1.read();
manz 9:946d400b2f13 526 sum_l += ain2.read();
manz 9:946d400b2f13 527 }
manz 9:946d400b2f13 528 value_r = sum_r/100;
manz 9:946d400b2f13 529 value_l = sum_l/100;
manz 9:946d400b2f13 530 //printf("Value: %f\r\n",value);
manz 9:946d400b2f13 531 if(value_r < 0.93 && on_r == 1){
manz 12:01250ea24795 532 //printf("Value right: %f\r\n",value_r);
manz 9:946d400b2f13 533 on_r = 0;
manz 9:946d400b2f13 534 }
manz 9:946d400b2f13 535 else if (value_r > 0.95 && on_r == 0){
manz 12:01250ea24795 536 //printf("Value right: %f\r\n",value_r);
manz 9:946d400b2f13 537 on_r = 1;
manz 9:946d400b2f13 538 counter_r--;
manz 12:01250ea24795 539 //printf("*** Right Counter is: %d\r\n",counter_r);
manz 9:946d400b2f13 540 }
manz 9:946d400b2f13 541 if(value_l < 0.93 && on_l == 1){
manz 12:01250ea24795 542 //printf("Value left: %f\r\n",value_l);
manz 9:946d400b2f13 543 on_l = 0;
manz 9:946d400b2f13 544 }
manz 9:946d400b2f13 545 else if (value_l > 0.95 && on_l == 0){
manz 12:01250ea24795 546 //printf("Value left: %f\r\n",value_l);
manz 9:946d400b2f13 547 on_l = 1;
manz 9:946d400b2f13 548 counter_l--;
manz 12:01250ea24795 549 //printf("*** Left Counter is: %d\r\n",counter_l);
manz 9:946d400b2f13 550 }
manz 9:946d400b2f13 551
manz 9:946d400b2f13 552 if(counter_l >= counter_r){
manz 9:946d400b2f13 553 counter_m = counter_r;
manz 9:946d400b2f13 554 }
manz 9:946d400b2f13 555 else{
manz 9:946d400b2f13 556 counter_m = counter_l;
manz 9:946d400b2f13 557 }
manz 12:01250ea24795 558 //deviation left from path
manz 12:01250ea24795 559 if(L.getAngle() - angle_init > 2 || (angle_init > 358 && L.getAngle() < 5 && (L.getAngle() + 360 - angle_init) > 2)){
manz 12:01250ea24795 560 servo_left();
manz 12:01250ea24795 561 serial.printf("Angle Difference: %f\r\n",L.getAngle() - angle_init);
manz 12:01250ea24795 562 while(fmod(abs(L.getAngle() - angle_init),360)> 1);
manz 12:01250ea24795 563 servo_f();
manz 12:01250ea24795 564 }
manz 12:01250ea24795 565 //deviation right from path
manz 12:01250ea24795 566 if(angle_init - L.getAngle() > 2 ){
manz 12:01250ea24795 567 servo_right();
manz 12:01250ea24795 568 while(fmod(abs(L.getAngle() - angle_init),360)> 1);
manz 12:01250ea24795 569 servo_f();
manz 12:01250ea24795 570 }
manz 12:01250ea24795 571 //printf("*** Distance to take is: %d\r\n",counter_m);
manz 9:946d400b2f13 572 }
manz 9:946d400b2f13 573 //servo_f();
manz 9:946d400b2f13 574 //wait(distance*time_factor);
manz 7:1bb3b5b66fe8 575 servo_stop();
manz 7:1bb3b5b66fe8 576 //serial.printf("Reached destination \r\n");
manz 7:1bb3b5b66fe8 577 //update pen and center when at target
manz 7:1bb3b5b66fe8 578 if(restore == 1){
manz 7:1bb3b5b66fe8 579 serial.printf("Restore previous positions \r\n");
manz 7:1bb3b5b66fe8 580 x_pen = x_pen_pr;
manz 7:1bb3b5b66fe8 581 y_pen = y_pen_pr;
manz 7:1bb3b5b66fe8 582 x_cent = x_cent_pr;
manz 7:1bb3b5b66fe8 583 y_cent = y_cent_pr;
manz 7:1bb3b5b66fe8 584 x_taro = x_taro_pr;
manz 7:1bb3b5b66fe8 585 y_taro = y_taro_pr;
manz 7:1bb3b5b66fe8 586 restore = 0;
manz 7:1bb3b5b66fe8 587 }
manz 7:1bb3b5b66fe8 588 else{
manz 7:1bb3b5b66fe8 589 x_pen = x_tar;
manz 7:1bb3b5b66fe8 590 y_pen = y_tar;
manz 7:1bb3b5b66fe8 591 x_cent = x_taro;
manz 7:1bb3b5b66fe8 592 y_cent = y_taro;
manz 7:1bb3b5b66fe8 593 x_taro = x_tar;
manz 7:1bb3b5b66fe8 594 y_taro = y_tar;
manz 7:1bb3b5b66fe8 595 }
manz 7:1bb3b5b66fe8 596 }
manz 0:83acd45a2405 597 serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen);
manz 7:1bb3b5b66fe8 598 serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent);
manz 5:1da4d4050306 599 }
manz 0:83acd45a2405 600 }
manz 0:83acd45a2405 601 }
manz 0:83acd45a2405 602
manz 5:1da4d4050306 603
manz 5:1da4d4050306 604
manz 0:83acd45a2405 605 int main()
manz 7:1bb3b5b66fe8 606 {
manz 7:1bb3b5b66fe8 607 L.reset();
manz 7:1bb3b5b66fe8 608 //L.servo(90);
manz 7:1bb3b5b66fe8 609 wait(1);
manz 7:1bb3b5b66fe8 610 serial.printf("Starting Dead-Reckoning\r\n");
manz 7:1bb3b5b66fe8 611 //deadreckoning();
manz 7:1bb3b5b66fe8 612 serial.printf("Starting the threads\r\n");
manz 0:83acd45a2405 613
manz 0:83acd45a2405 614 Thread bluetooth(bluetooth_thread);
manz 0:83acd45a2405 615 Thread move(move_thread);
manz 5:1da4d4050306 616 mover_id = move.gettid();
manz 5:1da4d4050306 617 bluetooth_id = bluetooth.gettid();
manz 0:83acd45a2405 618
manz 5:1da4d4050306 619 while(1){
manz 7:1bb3b5b66fe8 620 Thread::wait(3);
manz 5:1da4d4050306 621 if(which_thread == 0) bluetooth.signal_set(0x1);
manz 5:1da4d4050306 622 else move.signal_set(0x1);
manz 5:1da4d4050306 623 }
manz 0:83acd45a2405 624 }