SCRIBE / Mbed 2 deprecated SCRIBE_stepper

scribe robot working with stepper motors

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed

main.cpp@6:ca112c3083bb, 2016-05-12 (annotated)

Committer:
manz
Date:
Thu May 12 06:12:45 2016 +0000
Revision:
6:ca112c3083bb
Parent:
5:2c196f871096 
latest version;

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