The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of SCRIBE_stepper by
main.cpp@3:63aef644e6d2, 2016-04-23 (annotated)
- Committer:
- manz
- Date:
- Sat Apr 23 02:46:30 2016 +0000
- Revision:
- 3:63aef644e6d2
- Parent:
- 0:83acd45a2405
- Child:
- 5:1da4d4050306
bluetooth working with new app
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:83acd45a2405 | 7 | #include "stepper.h" |
manz | 0:83acd45a2405 | 8 | #include "rtos.h" |
manz | 0:83acd45a2405 | 9 | #include "localization.h"; |
manz | 0:83acd45a2405 | 10 | |
manz | 0:83acd45a2405 | 11 | Serial serial(USBTX, USBRX); |
manz | 0:83acd45a2405 | 12 | |
manz | 0:83acd45a2405 | 13 | SPI spi(p11, p12, p13); |
manz | 0:83acd45a2405 | 14 | DigitalInOut BLE_RDY(p21); |
manz | 0:83acd45a2405 | 15 | DigitalInOut BLE_REQ(p22); |
manz | 0:83acd45a2405 | 16 | DigitalInOut BLE_RESET(p23); |
manz | 0:83acd45a2405 | 17 | |
manz | 0:83acd45a2405 | 18 | unsigned char txbuf[16] = {0}; |
manz | 0:83acd45a2405 | 19 | unsigned char txlen = 0; |
manz | 0:83acd45a2405 | 20 | |
manz | 0:83acd45a2405 | 21 | // create peripheral |
manz | 0:83acd45a2405 | 22 | BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET); |
manz | 0:83acd45a2405 | 23 | |
manz | 0:83acd45a2405 | 24 | // create service w/ uuid |
manz | 0:83acd45a2405 | 25 | BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e"); |
manz | 0:83acd45a2405 | 26 | |
manz | 0:83acd45a2405 | 27 | // create characteristics |
manz | 0:83acd45a2405 | 28 | BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20); |
manz | 0:83acd45a2405 | 29 | BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20); |
manz | 0:83acd45a2405 | 30 | |
manz | 0:83acd45a2405 | 31 | unsigned int interval = 0; |
manz | 0:83acd45a2405 | 32 | unsigned char count_on = 0; |
manz | 0:83acd45a2405 | 33 | |
manz | 0:83acd45a2405 | 34 | // array to save the values of the user |
manz | 3:63aef644e6d2 | 35 | const int size = 400; |
manz | 0:83acd45a2405 | 36 | int path_x [size]; |
manz | 0:83acd45a2405 | 37 | int path_y [size]; |
manz | 0:83acd45a2405 | 38 | int counter = 0; |
manz | 0:83acd45a2405 | 39 | int start = 0; |
manz | 0:83acd45a2405 | 40 | |
manz | 3:63aef644e6d2 | 41 | const int shape_size = 10; |
manz | 3:63aef644e6d2 | 42 | int shape_x [shape_size]; |
manz | 3:63aef644e6d2 | 43 | int shape_y [shape_size]; |
manz | 3:63aef644e6d2 | 44 | |
manz | 3:63aef644e6d2 | 45 | int mode = -1; |
manz | 3:63aef644e6d2 | 46 | |
manz | 0:83acd45a2405 | 47 | //coordinates of the pen and the centre of SCRIBE at beginning |
manz | 0:83acd45a2405 | 48 | double x_pen = 0; |
manz | 0:83acd45a2405 | 49 | double y_pen = 0; |
manz | 0:83acd45a2405 | 50 | double x_cent = 0; |
manz | 0:83acd45a2405 | 51 | double y_cent = -1; |
manz | 0:83acd45a2405 | 52 | |
manz | 0:83acd45a2405 | 53 | //speed of SCRIBE |
manz | 0:83acd45a2405 | 54 | float speed = 0.3; |
manz | 0:83acd45a2405 | 55 | float t_factor = 0.15; |
manz | 0:83acd45a2405 | 56 | |
manz | 0:83acd45a2405 | 57 | |
manz | 0:83acd45a2405 | 58 | Semaphore one_slot(1); |
manz | 0:83acd45a2405 | 59 | |
manz | 3:63aef644e6d2 | 60 | localization L; |
manz | 0:83acd45a2405 | 61 | |
manz | 0:83acd45a2405 | 62 | void bluetooth_thread(void const *args){ |
manz | 0:83acd45a2405 | 63 | serial.printf("Serial begin!\r\n"); |
manz | 0:83acd45a2405 | 64 | int x,y,x_dir,y_dir; |
manz | 0:83acd45a2405 | 65 | |
manz | 0:83acd45a2405 | 66 | /*----- BLE Utility ---------------------------------------------*/ |
manz | 0:83acd45a2405 | 67 | // set advertised local name and service UUID |
manz | 0:83acd45a2405 | 68 | blePeripheral.setLocalName("BLE Shield"); |
manz | 0:83acd45a2405 | 69 | |
manz | 0:83acd45a2405 | 70 | blePeripheral.setAdvertisedServiceUuid(uartService.uuid()); |
manz | 0:83acd45a2405 | 71 | |
manz | 0:83acd45a2405 | 72 | // add service and characteristic |
manz | 0:83acd45a2405 | 73 | blePeripheral.addAttribute(uartService); |
manz | 0:83acd45a2405 | 74 | blePeripheral.addAttribute(rxCharacteristic); |
manz | 0:83acd45a2405 | 75 | blePeripheral.addAttribute(txCharacteristic); |
manz | 0:83acd45a2405 | 76 | |
manz | 0:83acd45a2405 | 77 | // begin initialization |
manz | 0:83acd45a2405 | 78 | blePeripheral.begin(); |
manz | 0:83acd45a2405 | 79 | /*---------------------------------------------------------------*/ |
manz | 0:83acd45a2405 | 80 | |
manz | 0:83acd45a2405 | 81 | //return value for move functions |
manz | 0:83acd45a2405 | 82 | int ret; |
manz | 0:83acd45a2405 | 83 | |
manz | 3:63aef644e6d2 | 84 | int width, length; |
manz | 3:63aef644e6d2 | 85 | |
manz | 0:83acd45a2405 | 86 | serial.printf("BLE UART Peripheral begin!\r\n"); |
manz | 0:83acd45a2405 | 87 | |
manz | 0:83acd45a2405 | 88 | while(1) |
manz | 0:83acd45a2405 | 89 | { |
manz | 0:83acd45a2405 | 90 | BLECentral central = blePeripheral.central(); |
manz | 0:83acd45a2405 | 91 | |
manz | 0:83acd45a2405 | 92 | if (central) |
manz | 0:83acd45a2405 | 93 | { |
manz | 0:83acd45a2405 | 94 | // central connected to peripheral |
manz | 0:83acd45a2405 | 95 | serial.printf("Connected to central\r\n"); |
manz | 0:83acd45a2405 | 96 | |
manz | 0:83acd45a2405 | 97 | while (central.connected()) |
manz | 0:83acd45a2405 | 98 | { |
manz | 0:83acd45a2405 | 99 | // central still connected to peripheral |
manz | 0:83acd45a2405 | 100 | if (rxCharacteristic.written()) |
manz | 0:83acd45a2405 | 101 | { |
manz | 0:83acd45a2405 | 102 | unsigned char rxlen = rxCharacteristic.valueLength(); |
manz | 0:83acd45a2405 | 103 | const unsigned char *val = rxCharacteristic.value(); |
manz | 3:63aef644e6d2 | 104 | serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); |
manz | 3:63aef644e6d2 | 105 | |
manz | 0:83acd45a2405 | 106 | unsigned char i = 0; |
manz | 0:83acd45a2405 | 107 | while(i<rxlen) |
manz | 0:83acd45a2405 | 108 | { |
manz | 0:83acd45a2405 | 109 | serial.printf("%d, ",val[i++]); |
manz | 0:83acd45a2405 | 110 | } |
manz | 3:63aef644e6d2 | 111 | |
manz | 0:83acd45a2405 | 112 | serial.printf("\r\n"); |
manz | 3:63aef644e6d2 | 113 | //determine mode of signal |
manz | 3:63aef644e6d2 | 114 | if(rxlen == 1){ |
manz | 3:63aef644e6d2 | 115 | if (mode != 13){ |
manz | 3:63aef644e6d2 | 116 | mode = (int) val[0]; |
manz | 3:63aef644e6d2 | 117 | } |
manz | 0:83acd45a2405 | 118 | } |
manz | 3:63aef644e6d2 | 119 | if(rxlen == 4){ |
manz | 3:63aef644e6d2 | 120 | //check if drawing mode |
manz | 3:63aef644e6d2 | 121 | if(mode != 4){ |
manz | 3:63aef644e6d2 | 122 | mode = 9; |
manz | 3:63aef644e6d2 | 123 | } |
manz | 3:63aef644e6d2 | 124 | x = (int) val[0]; |
manz | 3:63aef644e6d2 | 125 | y = (int) val[1]; |
manz | 3:63aef644e6d2 | 126 | x_dir = (int) val[2]; |
manz | 3:63aef644e6d2 | 127 | y_dir = (int) val[3]; |
manz | 3:63aef644e6d2 | 128 | |
manz | 3:63aef644e6d2 | 129 | // see if values are negative |
manz | 3:63aef644e6d2 | 130 | if(x_dir == 1){ |
manz | 3:63aef644e6d2 | 131 | x = -1*x; |
manz | 3:63aef644e6d2 | 132 | } |
manz | 3:63aef644e6d2 | 133 | if(y_dir == 1){ |
manz | 3:63aef644e6d2 | 134 | y = -1*y; |
manz | 3:63aef644e6d2 | 135 | } |
manz | 3:63aef644e6d2 | 136 | if (rxlen == 1){ |
manz | 3:63aef644e6d2 | 137 | mode = (int) val[0]; |
manz | 3:63aef644e6d2 | 138 | } |
manz | 3:63aef644e6d2 | 139 | // putting values into array |
manz | 3:63aef644e6d2 | 140 | serial.printf("try to put coordinates \r\n"); |
manz | 3:63aef644e6d2 | 141 | one_slot.wait(); |
manz | 3:63aef644e6d2 | 142 | if (counter == size){ |
manz | 3:63aef644e6d2 | 143 | counter = 0; |
manz | 3:63aef644e6d2 | 144 | } |
manz | 3:63aef644e6d2 | 145 | path_x[counter] = x; |
manz | 3:63aef644e6d2 | 146 | path_y[counter] = y; |
manz | 3:63aef644e6d2 | 147 | counter++; |
manz | 3:63aef644e6d2 | 148 | one_slot.release(); |
manz | 3:63aef644e6d2 | 149 | serial.printf("put coordinates \r\n"); |
manz | 0:83acd45a2405 | 150 | } |
manz | 0:83acd45a2405 | 151 | Thread::wait(100); |
manz | 0:83acd45a2405 | 152 | } |
manz | 0:83acd45a2405 | 153 | |
manz | 0:83acd45a2405 | 154 | if(serial.readable()) |
manz | 0:83acd45a2405 | 155 | { |
manz | 0:83acd45a2405 | 156 | if(!count_on) |
manz | 0:83acd45a2405 | 157 | { |
manz | 0:83acd45a2405 | 158 | count_on = 1; |
manz | 0:83acd45a2405 | 159 | } |
manz | 0:83acd45a2405 | 160 | interval = 0; |
manz | 0:83acd45a2405 | 161 | txbuf[txlen] = serial.getc(); |
manz | 0:83acd45a2405 | 162 | txlen++; |
manz | 0:83acd45a2405 | 163 | } |
manz | 0:83acd45a2405 | 164 | |
manz | 0:83acd45a2405 | 165 | if(count_on) // Count the interval after receiving a new char from terminate |
manz | 0:83acd45a2405 | 166 | { |
manz | 0:83acd45a2405 | 167 | interval++; |
manz | 0:83acd45a2405 | 168 | } |
manz | 0:83acd45a2405 | 169 | |
manz | 0:83acd45a2405 | 170 | if(interval == 10) // If there is no char available last the interval, send the received chars to central. |
manz | 0:83acd45a2405 | 171 | { |
manz | 0:83acd45a2405 | 172 | interval = 0; |
manz | 0:83acd45a2405 | 173 | count_on = 0; |
manz | 0:83acd45a2405 | 174 | serial.printf("Received from terminal: %d bytes\r\n", txlen); |
manz | 0:83acd45a2405 | 175 | txCharacteristic.setValue((const unsigned char *)txbuf, txlen); |
manz | 0:83acd45a2405 | 176 | txlen = 0; |
manz | 0:83acd45a2405 | 177 | } |
manz | 0:83acd45a2405 | 178 | } |
manz | 0:83acd45a2405 | 179 | // central disconnected |
manz | 0:83acd45a2405 | 180 | serial.printf("Disconnected from central\r\n"); |
manz | 0:83acd45a2405 | 181 | } |
manz | 0:83acd45a2405 | 182 | } |
manz | 0:83acd45a2405 | 183 | } |
manz | 0:83acd45a2405 | 184 | |
manz | 3:63aef644e6d2 | 185 | // incomplete |
manz | 3:63aef644e6d2 | 186 | int draw_circle(double radius){ |
manz | 3:63aef644e6d2 | 187 | float currAngle = L.getAngle(); |
manz | 3:63aef644e6d2 | 188 | if(radius >= 5){ |
manz | 3:63aef644e6d2 | 189 | int outer = (int) radius*0.6; |
manz | 3:63aef644e6d2 | 190 | int inner = (int) radius*0.2; |
manz | 3:63aef644e6d2 | 191 | } |
manz | 3:63aef644e6d2 | 192 | } |
manz | 3:63aef644e6d2 | 193 | |
manz | 0:83acd45a2405 | 194 | |
manz | 0:83acd45a2405 | 195 | void move_thread(void const *args){ |
manz | 0:83acd45a2405 | 196 | int angle; |
manz | 3:63aef644e6d2 | 197 | double length = 8; |
manz | 3:63aef644e6d2 | 198 | double width = 8; |
manz | 3:63aef644e6d2 | 199 | int radius = 5; |
manz | 0:83acd45a2405 | 200 | |
manz | 0:83acd45a2405 | 201 | Timer t; |
manz | 0:83acd45a2405 | 202 | t.start(); |
manz | 3:63aef644e6d2 | 203 | |
manz | 0:83acd45a2405 | 204 | L.reset(); |
manz | 0:83acd45a2405 | 205 | //initally put pen down |
manz | 0:83acd45a2405 | 206 | //L.servo(0); |
manz | 0:83acd45a2405 | 207 | |
manz | 0:83acd45a2405 | 208 | float currentAngle; |
manz | 0:83acd45a2405 | 209 | float targetAngle; |
manz | 0:83acd45a2405 | 210 | float startAngle; |
manz | 0:83acd45a2405 | 211 | float diffAngle; |
manz | 0:83acd45a2405 | 212 | |
manz | 0:83acd45a2405 | 213 | int control; |
manz | 3:63aef644e6d2 | 214 | int shape_count = 0; |
manz | 3:63aef644e6d2 | 215 | |
manz | 3:63aef644e6d2 | 216 | double draw_corr; |
manz | 0:83acd45a2405 | 217 | |
manz | 0:83acd45a2405 | 218 | double dot,a2,b2,c2; |
manz | 0:83acd45a2405 | 219 | double x_tar,y_tar; |
manz | 3:63aef644e6d2 | 220 | double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr; |
manz | 0:83acd45a2405 | 221 | double cosg,gamma,distance,wait_t; |
manz | 0:83acd45a2405 | 222 | double x_taro = 0; |
manz | 0:83acd45a2405 | 223 | double y_taro = 0; |
manz | 0:83acd45a2405 | 224 | serial.printf("Started move thread\r\n"); |
manz | 0:83acd45a2405 | 225 | int newval = 0; |
manz | 0:83acd45a2405 | 226 | |
manz | 0:83acd45a2405 | 227 | while(1){ |
manz | 3:63aef644e6d2 | 228 | // check what mode is present |
manz | 3:63aef644e6d2 | 229 | //rectangle/square |
manz | 3:63aef644e6d2 | 230 | if(mode == 0){ |
manz | 3:63aef644e6d2 | 231 | serial.printf("Draw rectangle \r\n"); |
manz | 3:63aef644e6d2 | 232 | //save old values and set initial |
manz | 3:63aef644e6d2 | 233 | x_pen_pr = x_pen; |
manz | 3:63aef644e6d2 | 234 | y_pen_pr = y_pen; |
manz | 3:63aef644e6d2 | 235 | x_cent_pr = x_cent; |
manz | 3:63aef644e6d2 | 236 | y_cent_pr = y_cent; |
manz | 3:63aef644e6d2 | 237 | x_pen = 0; |
manz | 3:63aef644e6d2 | 238 | y_pen = 0; |
manz | 3:63aef644e6d2 | 239 | x_cent = 0; |
manz | 3:63aef644e6d2 | 240 | y_cent = -1; |
manz | 3:63aef644e6d2 | 241 | |
manz | 3:63aef644e6d2 | 242 | //set values |
manz | 3:63aef644e6d2 | 243 | shape_x[3] = 0; |
manz | 3:63aef644e6d2 | 244 | shape_y[3] = length; |
manz | 3:63aef644e6d2 | 245 | shape_x[2] = width; |
manz | 3:63aef644e6d2 | 246 | shape_y[2] = length; |
manz | 3:63aef644e6d2 | 247 | shape_x[1] = width; |
manz | 3:63aef644e6d2 | 248 | shape_y[1] = 0; |
manz | 3:63aef644e6d2 | 249 | shape_x[0] = 0; |
manz | 3:63aef644e6d2 | 250 | shape_y[0] = 0; |
manz | 3:63aef644e6d2 | 251 | shape_count = 4; |
manz | 3:63aef644e6d2 | 252 | |
manz | 3:63aef644e6d2 | 253 | mode = 13; |
manz | 3:63aef644e6d2 | 254 | } |
manz | 3:63aef644e6d2 | 255 | else if(mode == 1){ |
manz | 3:63aef644e6d2 | 256 | serial.printf("Draw circle \r\n"); |
manz | 3:63aef644e6d2 | 257 | //call circle function |
manz | 3:63aef644e6d2 | 258 | control = draw_circle(radius); |
manz | 3:63aef644e6d2 | 259 | |
manz | 3:63aef644e6d2 | 260 | mode = -1; |
manz | 3:63aef644e6d2 | 261 | } |
manz | 3:63aef644e6d2 | 262 | else if(mode == 2){ |
manz | 3:63aef644e6d2 | 263 | serial.printf("Draw triangle \r\n"); |
manz | 3:63aef644e6d2 | 264 | //save old values and set initial |
manz | 3:63aef644e6d2 | 265 | x_pen_pr = x_pen; |
manz | 3:63aef644e6d2 | 266 | y_pen_pr = y_pen; |
manz | 3:63aef644e6d2 | 267 | x_cent_pr = x_cent; |
manz | 3:63aef644e6d2 | 268 | y_cent_pr = y_cent; |
manz | 3:63aef644e6d2 | 269 | x_pen = 0; |
manz | 3:63aef644e6d2 | 270 | y_pen = 0; |
manz | 3:63aef644e6d2 | 271 | x_cent = 0; |
manz | 3:63aef644e6d2 | 272 | y_cent = -1; |
manz | 3:63aef644e6d2 | 273 | |
manz | 3:63aef644e6d2 | 274 | //set values |
manz | 3:63aef644e6d2 | 275 | shape_x[2] = 0; |
manz | 3:63aef644e6d2 | 276 | shape_y[2] = length; |
manz | 3:63aef644e6d2 | 277 | shape_x[1] = width; |
manz | 3:63aef644e6d2 | 278 | shape_y[1] = 0; |
manz | 3:63aef644e6d2 | 279 | shape_x[0] = 0; |
manz | 3:63aef644e6d2 | 280 | shape_y[0] = 0; |
manz | 3:63aef644e6d2 | 281 | |
manz | 3:63aef644e6d2 | 282 | shape_count = 3; |
manz | 3:63aef644e6d2 | 283 | mode = 13; |
manz | 3:63aef644e6d2 | 284 | } |
manz | 3:63aef644e6d2 | 285 | else if(mode == 3){ |
manz | 3:63aef644e6d2 | 286 | serial.printf("Reset \r\n"); |
manz | 3:63aef644e6d2 | 287 | //set initial |
manz | 3:63aef644e6d2 | 288 | x_pen = 0; |
manz | 3:63aef644e6d2 | 289 | y_pen = 0; |
manz | 3:63aef644e6d2 | 290 | x_cent = 0; |
manz | 3:63aef644e6d2 | 291 | y_cent = -1; |
manz | 3:63aef644e6d2 | 292 | |
manz | 3:63aef644e6d2 | 293 | mode = -1; |
manz | 3:63aef644e6d2 | 294 | } |
manz | 3:63aef644e6d2 | 295 | else if (mode == 4){ |
manz | 3:63aef644e6d2 | 296 | serial.printf("Draw freely \r\n"); |
manz | 3:63aef644e6d2 | 297 | draw_corr = 0.2; |
manz | 3:63aef644e6d2 | 298 | } |
manz | 3:63aef644e6d2 | 299 | else if (mode == 9){ |
manz | 3:63aef644e6d2 | 300 | serial.printf("Draw coordinates \r\n"); |
manz | 3:63aef644e6d2 | 301 | draw_corr = 1; |
manz | 3:63aef644e6d2 | 302 | |
manz | 3:63aef644e6d2 | 303 | mode = -1; |
manz | 3:63aef644e6d2 | 304 | } |
manz | 3:63aef644e6d2 | 305 | |
manz | 3:63aef644e6d2 | 306 | // next coordinate is free drawing or coordinates |
manz | 3:63aef644e6d2 | 307 | if(shape_count == 0){ |
manz | 3:63aef644e6d2 | 308 | one_slot.wait(); |
manz | 3:63aef644e6d2 | 309 | if(counter != start){ |
manz | 3:63aef644e6d2 | 310 | if (start == size){ |
manz | 3:63aef644e6d2 | 311 | start = 0; |
manz | 3:63aef644e6d2 | 312 | } |
manz | 3:63aef644e6d2 | 313 | x_tar = path_x[start]; |
manz | 3:63aef644e6d2 | 314 | y_tar = path_y[start]; |
manz | 3:63aef644e6d2 | 315 | start++; |
manz | 3:63aef644e6d2 | 316 | newval = 1; |
manz | 0:83acd45a2405 | 317 | } |
manz | 3:63aef644e6d2 | 318 | one_slot.release(); |
manz | 0:83acd45a2405 | 319 | } |
manz | 3:63aef644e6d2 | 320 | // next coordinate is shape |
manz | 3:63aef644e6d2 | 321 | else{ |
manz | 3:63aef644e6d2 | 322 | shape_count = shape_count - 1; |
manz | 3:63aef644e6d2 | 323 | x_tar = shape_x[shape_count]; |
manz | 3:63aef644e6d2 | 324 | y_tar = shape_y[shape_count]; |
manz | 3:63aef644e6d2 | 325 | newval = 1; |
manz | 3:63aef644e6d2 | 326 | } |
manz | 0:83acd45a2405 | 327 | if(newval == 1){ |
manz | 0:83acd45a2405 | 328 | serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); |
manz | 0:83acd45a2405 | 329 | newval = 0; |
manz | 0:83acd45a2405 | 330 | |
manz | 0:83acd45a2405 | 331 | //compute angle and turn direction |
manz | 0:83acd45a2405 | 332 | a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent); |
manz | 0:83acd45a2405 | 333 | b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen); |
manz | 0:83acd45a2405 | 334 | c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent); |
manz | 0:83acd45a2405 | 335 | cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2)); |
manz | 0:83acd45a2405 | 336 | gamma = acos(cosg)/3.14159*180; |
manz | 0:83acd45a2405 | 337 | |
manz | 0:83acd45a2405 | 338 | dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); |
manz | 0:83acd45a2405 | 339 | |
manz | 0:83acd45a2405 | 340 | serial.printf("Angle: %f \r\n",gamma); |
manz | 0:83acd45a2405 | 341 | angle = ceil(180 - gamma); |
manz | 0:83acd45a2405 | 342 | serial.printf("Turning angle: %i \r\n",angle); |
manz | 0:83acd45a2405 | 343 | |
manz | 0:83acd45a2405 | 344 | //put pen up |
manz | 0:83acd45a2405 | 345 | //L.servo(30); |
manz | 0:83acd45a2405 | 346 | |
manz | 0:83acd45a2405 | 347 | currentAngle = L.getAngle(); |
manz | 0:83acd45a2405 | 348 | if(dot > 0){ |
manz | 0:83acd45a2405 | 349 | //serial.printf("Turn right \r\n"); |
manz | 0:83acd45a2405 | 350 | targetAngle = fmod(currentAngle+angle,360); |
manz | 0:83acd45a2405 | 351 | |
manz | 0:83acd45a2405 | 352 | while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ |
manz | 0:83acd45a2405 | 353 | control = step_right(); |
manz | 0:83acd45a2405 | 354 | serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); |
manz | 0:83acd45a2405 | 355 | } |
manz | 0:83acd45a2405 | 356 | //serial.printf("Reached target \r\n"); |
manz | 0:83acd45a2405 | 357 | } |
manz | 0:83acd45a2405 | 358 | else if(dot < 0){ |
manz | 0:83acd45a2405 | 359 | //serial.printf("Turn left \r\n"); |
manz | 0:83acd45a2405 | 360 | targetAngle = fmod(currentAngle-angle,360); |
manz | 0:83acd45a2405 | 361 | while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ |
manz | 0:83acd45a2405 | 362 | control = step_left(); |
manz | 0:83acd45a2405 | 363 | serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); |
manz | 0:83acd45a2405 | 364 | } |
manz | 0:83acd45a2405 | 365 | } |
manz | 0:83acd45a2405 | 366 | else{ |
manz | 0:83acd45a2405 | 367 | } |
manz | 0:83acd45a2405 | 368 | //put pen down again |
manz | 0:83acd45a2405 | 369 | //L.servo(0); |
manz | 0:83acd45a2405 | 370 | |
manz | 0:83acd45a2405 | 371 | startAngle = L.getAngle(); |
manz | 0:83acd45a2405 | 372 | serial.printf("Current angle: %f \r\n",startAngle); |
manz | 0:83acd45a2405 | 373 | //compute length of path til target |
manz | 0:83acd45a2405 | 374 | distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen)); |
manz | 0:83acd45a2405 | 375 | |
manz | 0:83acd45a2405 | 376 | //forward SCRIBE til target |
manz | 0:83acd45a2405 | 377 | wait_t = distance/speed*t_factor; |
manz | 0:83acd45a2405 | 378 | t.reset(); |
manz | 0:83acd45a2405 | 379 | while(t.read() < wait_t){ |
manz | 0:83acd45a2405 | 380 | control = step_f(); |
manz | 0:83acd45a2405 | 381 | } |
manz | 0:83acd45a2405 | 382 | serial.printf("Reached destination \r\n"); |
manz | 0:83acd45a2405 | 383 | |
manz | 0:83acd45a2405 | 384 | |
manz | 0:83acd45a2405 | 385 | //update pen and center when at target |
manz | 0:83acd45a2405 | 386 | x_pen = x_tar; |
manz | 0:83acd45a2405 | 387 | y_pen = y_tar; |
manz | 0:83acd45a2405 | 388 | serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen); |
manz | 0:83acd45a2405 | 389 | x_cent = x_taro; |
manz | 0:83acd45a2405 | 390 | y_cent = y_taro; |
manz | 0:83acd45a2405 | 391 | serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); |
manz | 0:83acd45a2405 | 392 | x_taro = x_tar; |
manz | 0:83acd45a2405 | 393 | y_taro = y_tar; |
manz | 0:83acd45a2405 | 394 | |
manz | 0:83acd45a2405 | 395 | } |
manz | 0:83acd45a2405 | 396 | Thread::wait(100); |
manz | 0:83acd45a2405 | 397 | } |
manz | 0:83acd45a2405 | 398 | } |
manz | 0:83acd45a2405 | 399 | |
manz | 0:83acd45a2405 | 400 | int main() |
manz | 0:83acd45a2405 | 401 | { |
manz | 0:83acd45a2405 | 402 | serial.printf("Starting the threads"); |
manz | 0:83acd45a2405 | 403 | |
manz | 0:83acd45a2405 | 404 | Thread bluetooth(bluetooth_thread); |
manz | 0:83acd45a2405 | 405 | Thread move(move_thread); |
manz | 0:83acd45a2405 | 406 | |
manz | 0:83acd45a2405 | 407 | Thread::wait(osWaitForever); |
manz | 0:83acd45a2405 | 408 | |
manz | 0:83acd45a2405 | 409 | } |