scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
main.cpp@4:1da4d4050306, 2016-04-25 (annotated)
- Committer:
- manz
- Date:
- Mon Apr 25 05:38:48 2016 +0000
- Revision:
- 4:1da4d4050306
- Parent:
- 2:63aef644e6d2
- Child:
- 5:2c196f871096
working with new thread system
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 | 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 | 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 | 4:1da4d4050306 | 34 | int which_thread = 0; |
manz | 4:1da4d4050306 | 35 | |
manz | 0:83acd45a2405 | 36 | // array to save the values of the user |
manz | 4:1da4d4050306 | 37 | const int size = 100; |
manz | 0:83acd45a2405 | 38 | int path_x [size]; |
manz | 0:83acd45a2405 | 39 | int path_y [size]; |
manz | 4:1da4d4050306 | 40 | int path_p [size]; |
manz | 0:83acd45a2405 | 41 | int counter = 0; |
manz | 0:83acd45a2405 | 42 | int start = 0; |
manz | 0:83acd45a2405 | 43 | |
manz | 2:63aef644e6d2 | 44 | const int shape_size = 10; |
manz | 2:63aef644e6d2 | 45 | int shape_x [shape_size]; |
manz | 2:63aef644e6d2 | 46 | int shape_y [shape_size]; |
manz | 2:63aef644e6d2 | 47 | |
manz | 4:1da4d4050306 | 48 | int mode = -1; //default mode is idle |
manz | 4:1da4d4050306 | 49 | int pmode = 1; //default pen is down |
manz | 2:63aef644e6d2 | 50 | |
manz | 0:83acd45a2405 | 51 | //coordinates of the pen and the centre of SCRIBE at beginning |
manz | 0:83acd45a2405 | 52 | double x_pen = 0; |
manz | 0:83acd45a2405 | 53 | double y_pen = 0; |
manz | 0:83acd45a2405 | 54 | double x_cent = 0; |
manz | 0:83acd45a2405 | 55 | double y_cent = -1; |
manz | 0:83acd45a2405 | 56 | |
manz | 0:83acd45a2405 | 57 | //speed of SCRIBE |
manz | 0:83acd45a2405 | 58 | float speed = 0.3; |
manz | 0:83acd45a2405 | 59 | float t_factor = 0.15; |
manz | 0:83acd45a2405 | 60 | |
manz | 0:83acd45a2405 | 61 | |
manz | 0:83acd45a2405 | 62 | Semaphore one_slot(1); |
manz | 0:83acd45a2405 | 63 | |
manz | 2:63aef644e6d2 | 64 | localization L; |
manz | 0:83acd45a2405 | 65 | |
manz | 4:1da4d4050306 | 66 | osThreadId bluetooth_id, mover_id; |
manz | 4:1da4d4050306 | 67 | |
manz | 0:83acd45a2405 | 68 | void bluetooth_thread(void const *args){ |
manz | 4:1da4d4050306 | 69 | bluetooth_id = Thread::gettid(); |
manz | 4:1da4d4050306 | 70 | |
manz | 0:83acd45a2405 | 71 | serial.printf("Serial begin!\r\n"); |
manz | 4:1da4d4050306 | 72 | |
manz | 4:1da4d4050306 | 73 | FILE *fp = fopen("/local/trial.m", "w"); |
manz | 4:1da4d4050306 | 74 | if (fp == NULL) { |
manz | 4:1da4d4050306 | 75 | serial.printf("ERROR: File open!\r\n"); |
manz | 4:1da4d4050306 | 76 | } |
manz | 4:1da4d4050306 | 77 | |
manz | 0:83acd45a2405 | 78 | int x,y,x_dir,y_dir; |
manz | 0:83acd45a2405 | 79 | |
manz | 0:83acd45a2405 | 80 | /*----- BLE Utility ---------------------------------------------*/ |
manz | 0:83acd45a2405 | 81 | // set advertised local name and service UUID |
manz | 0:83acd45a2405 | 82 | blePeripheral.setLocalName("BLE Shield"); |
manz | 0:83acd45a2405 | 83 | |
manz | 0:83acd45a2405 | 84 | blePeripheral.setAdvertisedServiceUuid(uartService.uuid()); |
manz | 0:83acd45a2405 | 85 | |
manz | 0:83acd45a2405 | 86 | // add service and characteristic |
manz | 0:83acd45a2405 | 87 | blePeripheral.addAttribute(uartService); |
manz | 0:83acd45a2405 | 88 | blePeripheral.addAttribute(rxCharacteristic); |
manz | 0:83acd45a2405 | 89 | blePeripheral.addAttribute(txCharacteristic); |
manz | 0:83acd45a2405 | 90 | |
manz | 0:83acd45a2405 | 91 | // begin initialization |
manz | 0:83acd45a2405 | 92 | blePeripheral.begin(); |
manz | 0:83acd45a2405 | 93 | /*---------------------------------------------------------------*/ |
manz | 0:83acd45a2405 | 94 | |
manz | 0:83acd45a2405 | 95 | serial.printf("BLE UART Peripheral begin!\r\n"); |
manz | 0:83acd45a2405 | 96 | |
manz | 0:83acd45a2405 | 97 | while(1) |
manz | 0:83acd45a2405 | 98 | { |
manz | 0:83acd45a2405 | 99 | BLECentral central = blePeripheral.central(); |
manz | 0:83acd45a2405 | 100 | |
manz | 0:83acd45a2405 | 101 | if (central) |
manz | 0:83acd45a2405 | 102 | { |
manz | 0:83acd45a2405 | 103 | // central connected to peripheral |
manz | 0:83acd45a2405 | 104 | serial.printf("Connected to central\r\n"); |
manz | 0:83acd45a2405 | 105 | while (central.connected()) |
manz | 4:1da4d4050306 | 106 | { |
manz | 4:1da4d4050306 | 107 | Thread::signal_wait(0x1); |
manz | 0:83acd45a2405 | 108 | // central still connected to peripheral |
manz | 0:83acd45a2405 | 109 | if (rxCharacteristic.written()) |
manz | 0:83acd45a2405 | 110 | { |
manz | 0:83acd45a2405 | 111 | unsigned char rxlen = rxCharacteristic.valueLength(); |
manz | 0:83acd45a2405 | 112 | const unsigned char *val = rxCharacteristic.value(); |
manz | 4:1da4d4050306 | 113 | /*serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); |
manz | 2:63aef644e6d2 | 114 | |
manz | 0:83acd45a2405 | 115 | unsigned char i = 0; |
manz | 0:83acd45a2405 | 116 | while(i<rxlen) |
manz | 0:83acd45a2405 | 117 | { |
manz | 0:83acd45a2405 | 118 | serial.printf("%d, ",val[i++]); |
manz | 0:83acd45a2405 | 119 | } |
manz | 2:63aef644e6d2 | 120 | |
manz | 4:1da4d4050306 | 121 | serial.printf("\r\n");*/ |
manz | 2:63aef644e6d2 | 122 | //determine mode of signal |
manz | 2:63aef644e6d2 | 123 | if(rxlen == 1){ |
manz | 4:1da4d4050306 | 124 | // inputs blocked until shape finished |
manz | 2:63aef644e6d2 | 125 | if (mode != 13){ |
manz | 2:63aef644e6d2 | 126 | mode = (int) val[0]; |
manz | 4:1da4d4050306 | 127 | if(mode != 4 && mode !=6){ |
manz | 4:1da4d4050306 | 128 | which_thread = 1; |
manz | 4:1da4d4050306 | 129 | osSignalClear(bluetooth_id,0x1); |
manz | 4:1da4d4050306 | 130 | } |
manz | 4:1da4d4050306 | 131 | } |
manz | 0:83acd45a2405 | 132 | } |
manz | 4:1da4d4050306 | 133 | if (mode == 6){ |
manz | 4:1da4d4050306 | 134 | serial.printf("Storing drawn coordinates\r\n"); |
manz | 4:1da4d4050306 | 135 | serial.printf("x_coord = ["); |
manz | 4:1da4d4050306 | 136 | for (int i = 0; i < size; i++) { |
manz | 4:1da4d4050306 | 137 | serial.printf("%i ", path_x[i]); |
manz | 4:1da4d4050306 | 138 | } |
manz | 4:1da4d4050306 | 139 | serial.printf("];\n"); |
manz | 4:1da4d4050306 | 140 | serial.printf("y_coord = ["); |
manz | 4:1da4d4050306 | 141 | for (int i = 0; i < size; i++) { |
manz | 4:1da4d4050306 | 142 | serial.printf("%i ", path_y[i]); |
manz | 4:1da4d4050306 | 143 | } |
manz | 4:1da4d4050306 | 144 | serial.printf("];\n"); |
manz | 4:1da4d4050306 | 145 | } |
manz | 4:1da4d4050306 | 146 | // stroke of pen finished |
manz | 4:1da4d4050306 | 147 | if (mode == 7){ |
manz | 4:1da4d4050306 | 148 | serial.printf("Other threads can proceed \r\n"); |
manz | 4:1da4d4050306 | 149 | osSignalClear(bluetooth_id,0x1); |
manz | 4:1da4d4050306 | 150 | which_thread = 1; |
manz | 4:1da4d4050306 | 151 | } |
manz | 4:1da4d4050306 | 152 | if(rxlen == 5){ |
manz | 4:1da4d4050306 | 153 | // in coordinates mode - accept negative coordinates |
manz | 4:1da4d4050306 | 154 | if(mode == 6){ |
manz | 2:63aef644e6d2 | 155 | mode = 9; |
manz | 4:1da4d4050306 | 156 | } |
manz | 4:1da4d4050306 | 157 | // in draw mode - convert coordinates |
manz | 4:1da4d4050306 | 158 | x = (int) val[0]*256 + val[1]; |
manz | 4:1da4d4050306 | 159 | y = (int) val[2]*256 + val[3]; |
manz | 2:63aef644e6d2 | 160 | |
manz | 2:63aef644e6d2 | 161 | // putting values into array |
manz | 4:1da4d4050306 | 162 | //serial.printf("try to put coordinates \r\n"); |
manz | 2:63aef644e6d2 | 163 | one_slot.wait(); |
manz | 2:63aef644e6d2 | 164 | if (counter == size){ |
manz | 4:1da4d4050306 | 165 | serial.printf("Overwriting values \r\n"); |
manz | 2:63aef644e6d2 | 166 | counter = 0; |
manz | 2:63aef644e6d2 | 167 | } |
manz | 2:63aef644e6d2 | 168 | path_x[counter] = x; |
manz | 2:63aef644e6d2 | 169 | path_y[counter] = y; |
manz | 4:1da4d4050306 | 170 | |
manz | 4:1da4d4050306 | 171 | //check if pen needs to be up or down |
manz | 4:1da4d4050306 | 172 | path_p[counter] = pmode; |
manz | 4:1da4d4050306 | 173 | |
manz | 2:63aef644e6d2 | 174 | counter++; |
manz | 2:63aef644e6d2 | 175 | one_slot.release(); |
manz | 4:1da4d4050306 | 176 | if(mode == 9){ |
manz | 4:1da4d4050306 | 177 | which_thread = 1; |
manz | 4:1da4d4050306 | 178 | osSignalClear(bluetooth_id,0x1); |
manz | 4:1da4d4050306 | 179 | } |
manz | 4:1da4d4050306 | 180 | //serial.printf("put coordinates \r\n"); |
manz | 0:83acd45a2405 | 181 | } |
manz | 0:83acd45a2405 | 182 | } |
manz | 0:83acd45a2405 | 183 | |
manz | 0:83acd45a2405 | 184 | if(serial.readable()) |
manz | 0:83acd45a2405 | 185 | { |
manz | 0:83acd45a2405 | 186 | if(!count_on) |
manz | 0:83acd45a2405 | 187 | { |
manz | 0:83acd45a2405 | 188 | count_on = 1; |
manz | 0:83acd45a2405 | 189 | } |
manz | 0:83acd45a2405 | 190 | interval = 0; |
manz | 0:83acd45a2405 | 191 | txbuf[txlen] = serial.getc(); |
manz | 0:83acd45a2405 | 192 | txlen++; |
manz | 0:83acd45a2405 | 193 | } |
manz | 0:83acd45a2405 | 194 | |
manz | 0:83acd45a2405 | 195 | if(count_on) // Count the interval after receiving a new char from terminate |
manz | 0:83acd45a2405 | 196 | { |
manz | 0:83acd45a2405 | 197 | interval++; |
manz | 0:83acd45a2405 | 198 | } |
manz | 0:83acd45a2405 | 199 | |
manz | 0:83acd45a2405 | 200 | if(interval == 10) // If there is no char available last the interval, send the received chars to central. |
manz | 0:83acd45a2405 | 201 | { |
manz | 0:83acd45a2405 | 202 | interval = 0; |
manz | 0:83acd45a2405 | 203 | count_on = 0; |
manz | 4:1da4d4050306 | 204 | //serial.printf("Received from terminal: %d bytes\r\n", txlen); |
manz | 0:83acd45a2405 | 205 | txCharacteristic.setValue((const unsigned char *)txbuf, txlen); |
manz | 0:83acd45a2405 | 206 | txlen = 0; |
manz | 0:83acd45a2405 | 207 | } |
manz | 0:83acd45a2405 | 208 | } |
manz | 4:1da4d4050306 | 209 | |
manz | 0:83acd45a2405 | 210 | // central disconnected |
manz | 0:83acd45a2405 | 211 | serial.printf("Disconnected from central\r\n"); |
manz | 0:83acd45a2405 | 212 | } |
manz | 0:83acd45a2405 | 213 | } |
manz | 0:83acd45a2405 | 214 | } |
manz | 0:83acd45a2405 | 215 | |
manz | 4:1da4d4050306 | 216 | |
manz | 2:63aef644e6d2 | 217 | // incomplete |
manz | 2:63aef644e6d2 | 218 | int draw_circle(double radius){ |
manz | 2:63aef644e6d2 | 219 | float currAngle = L.getAngle(); |
manz | 2:63aef644e6d2 | 220 | if(radius >= 5){ |
manz | 2:63aef644e6d2 | 221 | int outer = (int) radius*0.6; |
manz | 2:63aef644e6d2 | 222 | int inner = (int) radius*0.2; |
manz | 2:63aef644e6d2 | 223 | } |
manz | 2:63aef644e6d2 | 224 | } |
manz | 2:63aef644e6d2 | 225 | |
manz | 0:83acd45a2405 | 226 | |
manz | 0:83acd45a2405 | 227 | void move_thread(void const *args){ |
manz | 4:1da4d4050306 | 228 | |
manz | 0:83acd45a2405 | 229 | int angle; |
manz | 4:1da4d4050306 | 230 | double length = 40; |
manz | 4:1da4d4050306 | 231 | double width = 40; |
manz | 2:63aef644e6d2 | 232 | int radius = 5; |
manz | 0:83acd45a2405 | 233 | |
manz | 0:83acd45a2405 | 234 | Timer t; |
manz | 0:83acd45a2405 | 235 | t.start(); |
manz | 2:63aef644e6d2 | 236 | |
manz | 0:83acd45a2405 | 237 | L.reset(); |
manz | 0:83acd45a2405 | 238 | //initally put pen down |
manz | 0:83acd45a2405 | 239 | //L.servo(0); |
manz | 0:83acd45a2405 | 240 | |
manz | 0:83acd45a2405 | 241 | float currentAngle; |
manz | 0:83acd45a2405 | 242 | float targetAngle; |
manz | 0:83acd45a2405 | 243 | float startAngle; |
manz | 0:83acd45a2405 | 244 | float diffAngle; |
manz | 0:83acd45a2405 | 245 | |
manz | 4:1da4d4050306 | 246 | int steps; |
manz | 4:1da4d4050306 | 247 | |
manz | 0:83acd45a2405 | 248 | int control; |
manz | 2:63aef644e6d2 | 249 | int shape_count = 0; |
manz | 4:1da4d4050306 | 250 | int p_mode; |
manz | 4:1da4d4050306 | 251 | int first = 0; |
manz | 2:63aef644e6d2 | 252 | |
manz | 4:1da4d4050306 | 253 | double draw_corr = 5; |
manz | 0:83acd45a2405 | 254 | |
manz | 0:83acd45a2405 | 255 | double dot,a2,b2,c2; |
manz | 0:83acd45a2405 | 256 | double x_tar,y_tar; |
manz | 2:63aef644e6d2 | 257 | double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr; |
manz | 0:83acd45a2405 | 258 | double cosg,gamma,distance,wait_t; |
manz | 0:83acd45a2405 | 259 | double x_taro = 0; |
manz | 0:83acd45a2405 | 260 | double y_taro = 0; |
manz | 0:83acd45a2405 | 261 | serial.printf("Started move thread\r\n"); |
manz | 0:83acd45a2405 | 262 | int newval = 0; |
manz | 4:1da4d4050306 | 263 | |
manz | 0:83acd45a2405 | 264 | while(1){ |
manz | 2:63aef644e6d2 | 265 | // check what mode is present |
manz | 4:1da4d4050306 | 266 | serial.printf("Cylce \r\n"); |
manz | 4:1da4d4050306 | 267 | Thread::signal_wait(0x1); |
manz | 4:1da4d4050306 | 268 | if(mode == -1){ |
manz | 4:1da4d4050306 | 269 | serial.printf("here ends"); |
manz | 4:1da4d4050306 | 270 | osSignalClear(mover_id,0x1); |
manz | 4:1da4d4050306 | 271 | which_thread = 0; |
manz | 4:1da4d4050306 | 272 | } |
manz | 2:63aef644e6d2 | 273 | //rectangle/square |
manz | 4:1da4d4050306 | 274 | else if(mode == 0){ |
manz | 2:63aef644e6d2 | 275 | serial.printf("Draw rectangle \r\n"); |
manz | 2:63aef644e6d2 | 276 | //save old values and set initial |
manz | 2:63aef644e6d2 | 277 | x_pen_pr = x_pen; |
manz | 2:63aef644e6d2 | 278 | y_pen_pr = y_pen; |
manz | 2:63aef644e6d2 | 279 | x_cent_pr = x_cent; |
manz | 2:63aef644e6d2 | 280 | y_cent_pr = y_cent; |
manz | 2:63aef644e6d2 | 281 | x_pen = 0; |
manz | 2:63aef644e6d2 | 282 | y_pen = 0; |
manz | 2:63aef644e6d2 | 283 | x_cent = 0; |
manz | 2:63aef644e6d2 | 284 | y_cent = -1; |
manz | 4:1da4d4050306 | 285 | x_taro = 0; |
manz | 4:1da4d4050306 | 286 | y_taro = 0; |
manz | 2:63aef644e6d2 | 287 | |
manz | 2:63aef644e6d2 | 288 | //set values |
manz | 2:63aef644e6d2 | 289 | shape_x[3] = 0; |
manz | 2:63aef644e6d2 | 290 | shape_y[3] = length; |
manz | 2:63aef644e6d2 | 291 | shape_x[2] = width; |
manz | 2:63aef644e6d2 | 292 | shape_y[2] = length; |
manz | 2:63aef644e6d2 | 293 | shape_x[1] = width; |
manz | 2:63aef644e6d2 | 294 | shape_y[1] = 0; |
manz | 2:63aef644e6d2 | 295 | shape_x[0] = 0; |
manz | 2:63aef644e6d2 | 296 | shape_y[0] = 0; |
manz | 2:63aef644e6d2 | 297 | shape_count = 4; |
manz | 2:63aef644e6d2 | 298 | |
manz | 2:63aef644e6d2 | 299 | mode = 13; |
manz | 2:63aef644e6d2 | 300 | } |
manz | 2:63aef644e6d2 | 301 | else if(mode == 1){ |
manz | 2:63aef644e6d2 | 302 | serial.printf("Draw circle \r\n"); |
manz | 2:63aef644e6d2 | 303 | //call circle function |
manz | 2:63aef644e6d2 | 304 | control = draw_circle(radius); |
manz | 2:63aef644e6d2 | 305 | |
manz | 2:63aef644e6d2 | 306 | mode = -1; |
manz | 2:63aef644e6d2 | 307 | } |
manz | 2:63aef644e6d2 | 308 | else if(mode == 2){ |
manz | 2:63aef644e6d2 | 309 | serial.printf("Draw triangle \r\n"); |
manz | 2:63aef644e6d2 | 310 | //save old values and set initial |
manz | 2:63aef644e6d2 | 311 | x_pen_pr = x_pen; |
manz | 2:63aef644e6d2 | 312 | y_pen_pr = y_pen; |
manz | 2:63aef644e6d2 | 313 | x_cent_pr = x_cent; |
manz | 2:63aef644e6d2 | 314 | y_cent_pr = y_cent; |
manz | 2:63aef644e6d2 | 315 | x_pen = 0; |
manz | 2:63aef644e6d2 | 316 | y_pen = 0; |
manz | 2:63aef644e6d2 | 317 | x_cent = 0; |
manz | 2:63aef644e6d2 | 318 | y_cent = -1; |
manz | 4:1da4d4050306 | 319 | x_taro = 0; |
manz | 4:1da4d4050306 | 320 | y_taro = 0; |
manz | 2:63aef644e6d2 | 321 | |
manz | 2:63aef644e6d2 | 322 | //set values |
manz | 2:63aef644e6d2 | 323 | shape_x[2] = 0; |
manz | 2:63aef644e6d2 | 324 | shape_y[2] = length; |
manz | 2:63aef644e6d2 | 325 | shape_x[1] = width; |
manz | 2:63aef644e6d2 | 326 | shape_y[1] = 0; |
manz | 2:63aef644e6d2 | 327 | shape_x[0] = 0; |
manz | 2:63aef644e6d2 | 328 | shape_y[0] = 0; |
manz | 2:63aef644e6d2 | 329 | |
manz | 2:63aef644e6d2 | 330 | shape_count = 3; |
manz | 2:63aef644e6d2 | 331 | mode = 13; |
manz | 2:63aef644e6d2 | 332 | } |
manz | 2:63aef644e6d2 | 333 | else if(mode == 3){ |
manz | 2:63aef644e6d2 | 334 | serial.printf("Reset \r\n"); |
manz | 2:63aef644e6d2 | 335 | //set initial |
manz | 2:63aef644e6d2 | 336 | x_pen = 0; |
manz | 2:63aef644e6d2 | 337 | y_pen = 0; |
manz | 2:63aef644e6d2 | 338 | x_cent = 0; |
manz | 2:63aef644e6d2 | 339 | y_cent = -1; |
manz | 2:63aef644e6d2 | 340 | |
manz | 2:63aef644e6d2 | 341 | mode = -1; |
manz | 2:63aef644e6d2 | 342 | } |
manz | 4:1da4d4050306 | 343 | else if (mode == 7){ |
manz | 4:1da4d4050306 | 344 | if (first == 0){ |
manz | 4:1da4d4050306 | 345 | serial.printf("Draw freely \r\n"); |
manz | 4:1da4d4050306 | 346 | draw_corr = 1; |
manz | 4:1da4d4050306 | 347 | } |
manz | 4:1da4d4050306 | 348 | first = 1; |
manz | 4:1da4d4050306 | 349 | |
manz | 2:63aef644e6d2 | 350 | } |
manz | 2:63aef644e6d2 | 351 | else if (mode == 9){ |
manz | 2:63aef644e6d2 | 352 | serial.printf("Draw coordinates \r\n"); |
manz | 4:1da4d4050306 | 353 | draw_corr = 5; |
manz | 2:63aef644e6d2 | 354 | mode = -1; |
manz | 2:63aef644e6d2 | 355 | } |
manz | 2:63aef644e6d2 | 356 | |
manz | 2:63aef644e6d2 | 357 | // next coordinate is free drawing or coordinates |
manz | 2:63aef644e6d2 | 358 | if(shape_count == 0){ |
manz | 2:63aef644e6d2 | 359 | one_slot.wait(); |
manz | 2:63aef644e6d2 | 360 | if(counter != start){ |
manz | 2:63aef644e6d2 | 361 | if (start == size){ |
manz | 2:63aef644e6d2 | 362 | start = 0; |
manz | 2:63aef644e6d2 | 363 | } |
manz | 4:1da4d4050306 | 364 | x_tar = (double) path_x[start]*draw_corr; |
manz | 4:1da4d4050306 | 365 | y_tar = (double) path_y[start]*draw_corr; |
manz | 4:1da4d4050306 | 366 | p_mode = path_p[start]; |
manz | 2:63aef644e6d2 | 367 | start++; |
manz | 4:1da4d4050306 | 368 | if(start == counter) mode = -1; |
manz | 2:63aef644e6d2 | 369 | newval = 1; |
manz | 0:83acd45a2405 | 370 | } |
manz | 2:63aef644e6d2 | 371 | one_slot.release(); |
manz | 0:83acd45a2405 | 372 | } |
manz | 2:63aef644e6d2 | 373 | // next coordinate is shape |
manz | 2:63aef644e6d2 | 374 | else{ |
manz | 2:63aef644e6d2 | 375 | shape_count = shape_count - 1; |
manz | 4:1da4d4050306 | 376 | x_tar = (double) shape_x[shape_count]; |
manz | 4:1da4d4050306 | 377 | y_tar = (double) shape_y[shape_count]; |
manz | 4:1da4d4050306 | 378 | p_mode = 1; |
manz | 2:63aef644e6d2 | 379 | newval = 1; |
manz | 4:1da4d4050306 | 380 | |
manz | 4:1da4d4050306 | 381 | //last move -> unblock input |
manz | 4:1da4d4050306 | 382 | if(shape_count == 0){ |
manz | 4:1da4d4050306 | 383 | mode = -1; |
manz | 4:1da4d4050306 | 384 | } |
manz | 2:63aef644e6d2 | 385 | } |
manz | 0:83acd45a2405 | 386 | if(newval == 1){ |
manz | 0:83acd45a2405 | 387 | serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); |
manz | 0:83acd45a2405 | 388 | newval = 0; |
manz | 0:83acd45a2405 | 389 | |
manz | 0:83acd45a2405 | 390 | //compute angle and turn direction |
manz | 0:83acd45a2405 | 391 | a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent); |
manz | 0:83acd45a2405 | 392 | b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen); |
manz | 0:83acd45a2405 | 393 | c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent); |
manz | 0:83acd45a2405 | 394 | cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2)); |
manz | 0:83acd45a2405 | 395 | gamma = acos(cosg)/3.14159*180; |
manz | 0:83acd45a2405 | 396 | |
manz | 0:83acd45a2405 | 397 | dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); |
manz | 0:83acd45a2405 | 398 | |
manz | 4:1da4d4050306 | 399 | //serial.printf("Angle: %f \r\n",gamma); |
manz | 0:83acd45a2405 | 400 | angle = ceil(180 - gamma); |
manz | 0:83acd45a2405 | 401 | serial.printf("Turning angle: %i \r\n",angle); |
manz | 0:83acd45a2405 | 402 | |
manz | 4:1da4d4050306 | 403 | //put pen down |
manz | 4:1da4d4050306 | 404 | if (p_mode == 1){ |
manz | 4:1da4d4050306 | 405 | //serial.printf("Pen down \r\n"); |
manz | 4:1da4d4050306 | 406 | L.servo(100); |
manz | 4:1da4d4050306 | 407 | } |
manz | 0:83acd45a2405 | 408 | //put pen up |
manz | 4:1da4d4050306 | 409 | else if (p_mode == 0){ |
manz | 4:1da4d4050306 | 410 | //serial.printf("Pen up \r\n"); |
manz | 4:1da4d4050306 | 411 | L.servo(90); |
manz | 4:1da4d4050306 | 412 | } |
manz | 0:83acd45a2405 | 413 | |
manz | 0:83acd45a2405 | 414 | currentAngle = L.getAngle(); |
manz | 0:83acd45a2405 | 415 | if(dot > 0){ |
manz | 0:83acd45a2405 | 416 | //serial.printf("Turn right \r\n"); |
manz | 0:83acd45a2405 | 417 | targetAngle = fmod(currentAngle+angle,360); |
manz | 0:83acd45a2405 | 418 | |
manz | 0:83acd45a2405 | 419 | while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ |
manz | 0:83acd45a2405 | 420 | control = step_right(); |
manz | 4:1da4d4050306 | 421 | //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360)); |
manz | 0:83acd45a2405 | 422 | } |
manz | 0:83acd45a2405 | 423 | //serial.printf("Reached target \r\n"); |
manz | 0:83acd45a2405 | 424 | } |
manz | 0:83acd45a2405 | 425 | else if(dot < 0){ |
manz | 0:83acd45a2405 | 426 | //serial.printf("Turn left \r\n"); |
manz | 0:83acd45a2405 | 427 | targetAngle = fmod(currentAngle-angle,360); |
manz | 0:83acd45a2405 | 428 | while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ |
manz | 0:83acd45a2405 | 429 | control = step_left(); |
manz | 4:1da4d4050306 | 430 | //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360)); |
manz | 0:83acd45a2405 | 431 | } |
manz | 0:83acd45a2405 | 432 | } |
manz | 0:83acd45a2405 | 433 | //compute length of path til target |
manz | 0:83acd45a2405 | 434 | distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen)); |
manz | 0:83acd45a2405 | 435 | |
manz | 0:83acd45a2405 | 436 | //forward SCRIBE til target |
manz | 4:1da4d4050306 | 437 | steps = (int) distance; |
manz | 4:1da4d4050306 | 438 | for(int i = 0; i< steps; i++){ |
manz | 0:83acd45a2405 | 439 | control = step_f(); |
manz | 0:83acd45a2405 | 440 | } |
manz | 4:1da4d4050306 | 441 | //serial.printf("Reached destination \r\n"); |
manz | 0:83acd45a2405 | 442 | |
manz | 0:83acd45a2405 | 443 | //update pen and center when at target |
manz | 0:83acd45a2405 | 444 | x_pen = x_tar; |
manz | 0:83acd45a2405 | 445 | y_pen = y_tar; |
manz | 0:83acd45a2405 | 446 | serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen); |
manz | 0:83acd45a2405 | 447 | x_cent = x_taro; |
manz | 0:83acd45a2405 | 448 | y_cent = y_taro; |
manz | 0:83acd45a2405 | 449 | serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); |
manz | 0:83acd45a2405 | 450 | x_taro = x_tar; |
manz | 0:83acd45a2405 | 451 | y_taro = y_tar; |
manz | 0:83acd45a2405 | 452 | |
manz | 4:1da4d4050306 | 453 | } |
manz | 0:83acd45a2405 | 454 | } |
manz | 0:83acd45a2405 | 455 | } |
manz | 0:83acd45a2405 | 456 | |
manz | 4:1da4d4050306 | 457 | |
manz | 4:1da4d4050306 | 458 | |
manz | 0:83acd45a2405 | 459 | int main() |
manz | 0:83acd45a2405 | 460 | { |
manz | 0:83acd45a2405 | 461 | serial.printf("Starting the threads"); |
manz | 0:83acd45a2405 | 462 | |
manz | 0:83acd45a2405 | 463 | Thread bluetooth(bluetooth_thread); |
manz | 0:83acd45a2405 | 464 | Thread move(move_thread); |
manz | 4:1da4d4050306 | 465 | mover_id = move.gettid(); |
manz | 4:1da4d4050306 | 466 | bluetooth_id = bluetooth.gettid(); |
manz | 0:83acd45a2405 | 467 | |
manz | 4:1da4d4050306 | 468 | while(1){ |
manz | 4:1da4d4050306 | 469 | if(which_thread == 0) bluetooth.signal_set(0x1); |
manz | 4:1da4d4050306 | 470 | else move.signal_set(0x1); |
manz | 4:1da4d4050306 | 471 | } |
manz | 0:83acd45a2405 | 472 | } |