![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of SCRIBE_stepper by
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?
User | Revision | Line number | New 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 | } |