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@8:4d7b2dbdb694, 2016-05-05 (annotated)

Committer:
nibab
Date:
Thu May 05 01:43:03 2016 +0000
Revision:
8:4d7b2dbdb694
Parent:
7:1bb3b5b66fe8
Child:
9:946d400b2f13
Adjust servo arguments and the timing coefficient

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