![](/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@0:83acd45a2405, 2016-04-21 (annotated)
- Committer:
- manz
- Date:
- Thu Apr 21 22:01:19 2016 +0000
- Revision:
- 0:83acd45a2405
- Child:
- 3:63aef644e6d2
stepper version of SCRIBE
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 | 0:83acd45a2405 | 35 | const int size = 40; |
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 | 0:83acd45a2405 | 41 | //coordinates of the pen and the centre of SCRIBE at beginning |
manz | 0:83acd45a2405 | 42 | double x_pen = 0; |
manz | 0:83acd45a2405 | 43 | double y_pen = 0; |
manz | 0:83acd45a2405 | 44 | double x_cent = 0; |
manz | 0:83acd45a2405 | 45 | double y_cent = -1; |
manz | 0:83acd45a2405 | 46 | |
manz | 0:83acd45a2405 | 47 | //speed of SCRIBE |
manz | 0:83acd45a2405 | 48 | float speed = 0.3; |
manz | 0:83acd45a2405 | 49 | float t_factor = 0.15; |
manz | 0:83acd45a2405 | 50 | |
manz | 0:83acd45a2405 | 51 | |
manz | 0:83acd45a2405 | 52 | Semaphore one_slot(1); |
manz | 0:83acd45a2405 | 53 | |
manz | 0:83acd45a2405 | 54 | |
manz | 0:83acd45a2405 | 55 | void bluetooth_thread(void const *args){ |
manz | 0:83acd45a2405 | 56 | serial.printf("Serial begin!\r\n"); |
manz | 0:83acd45a2405 | 57 | int x,y,x_dir,y_dir; |
manz | 0:83acd45a2405 | 58 | |
manz | 0:83acd45a2405 | 59 | /*----- BLE Utility ---------------------------------------------*/ |
manz | 0:83acd45a2405 | 60 | // set advertised local name and service UUID |
manz | 0:83acd45a2405 | 61 | blePeripheral.setLocalName("BLE Shield"); |
manz | 0:83acd45a2405 | 62 | |
manz | 0:83acd45a2405 | 63 | blePeripheral.setAdvertisedServiceUuid(uartService.uuid()); |
manz | 0:83acd45a2405 | 64 | |
manz | 0:83acd45a2405 | 65 | // add service and characteristic |
manz | 0:83acd45a2405 | 66 | blePeripheral.addAttribute(uartService); |
manz | 0:83acd45a2405 | 67 | blePeripheral.addAttribute(rxCharacteristic); |
manz | 0:83acd45a2405 | 68 | blePeripheral.addAttribute(txCharacteristic); |
manz | 0:83acd45a2405 | 69 | |
manz | 0:83acd45a2405 | 70 | // begin initialization |
manz | 0:83acd45a2405 | 71 | blePeripheral.begin(); |
manz | 0:83acd45a2405 | 72 | /*---------------------------------------------------------------*/ |
manz | 0:83acd45a2405 | 73 | |
manz | 0:83acd45a2405 | 74 | //return value for move functions |
manz | 0:83acd45a2405 | 75 | int ret; |
manz | 0:83acd45a2405 | 76 | |
manz | 0:83acd45a2405 | 77 | serial.printf("BLE UART Peripheral begin!\r\n"); |
manz | 0:83acd45a2405 | 78 | |
manz | 0:83acd45a2405 | 79 | while(1) |
manz | 0:83acd45a2405 | 80 | { |
manz | 0:83acd45a2405 | 81 | BLECentral central = blePeripheral.central(); |
manz | 0:83acd45a2405 | 82 | |
manz | 0:83acd45a2405 | 83 | if (central) |
manz | 0:83acd45a2405 | 84 | { |
manz | 0:83acd45a2405 | 85 | // central connected to peripheral |
manz | 0:83acd45a2405 | 86 | serial.printf("Connected to central\r\n"); |
manz | 0:83acd45a2405 | 87 | |
manz | 0:83acd45a2405 | 88 | while (central.connected()) |
manz | 0:83acd45a2405 | 89 | { |
manz | 0:83acd45a2405 | 90 | // central still connected to peripheral |
manz | 0:83acd45a2405 | 91 | if (rxCharacteristic.written()) |
manz | 0:83acd45a2405 | 92 | { |
manz | 0:83acd45a2405 | 93 | unsigned char rxlen = rxCharacteristic.valueLength(); |
manz | 0:83acd45a2405 | 94 | const unsigned char *val = rxCharacteristic.value(); |
manz | 0:83acd45a2405 | 95 | serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); |
manz | 0:83acd45a2405 | 96 | unsigned char i = 0; |
manz | 0:83acd45a2405 | 97 | while(i<rxlen) |
manz | 0:83acd45a2405 | 98 | { |
manz | 0:83acd45a2405 | 99 | serial.printf("%d, ",val[i++]); |
manz | 0:83acd45a2405 | 100 | } |
manz | 0:83acd45a2405 | 101 | serial.printf("\r\n"); |
manz | 0:83acd45a2405 | 102 | x = (int) val[0]; |
manz | 0:83acd45a2405 | 103 | y = (int) val[1]; |
manz | 0:83acd45a2405 | 104 | x_dir = (int) val[2]; |
manz | 0:83acd45a2405 | 105 | y_dir = (int) val[3]; |
manz | 0:83acd45a2405 | 106 | |
manz | 0:83acd45a2405 | 107 | // see if values are negative |
manz | 0:83acd45a2405 | 108 | if(x_dir == 1){ |
manz | 0:83acd45a2405 | 109 | x = -1*x; |
manz | 0:83acd45a2405 | 110 | } |
manz | 0:83acd45a2405 | 111 | if(y_dir == 1){ |
manz | 0:83acd45a2405 | 112 | y = -1*y; |
manz | 0:83acd45a2405 | 113 | } |
manz | 0:83acd45a2405 | 114 | // go forward |
manz | 0:83acd45a2405 | 115 | if(x == 0 && y == 4){ |
manz | 0:83acd45a2405 | 116 | serial.printf("Go forward \r\n"); |
manz | 0:83acd45a2405 | 117 | } |
manz | 0:83acd45a2405 | 118 | // go backward |
manz | 0:83acd45a2405 | 119 | else if(x == 0 && y == -4){ |
manz | 0:83acd45a2405 | 120 | serial.printf("Go backward \r\n"); |
manz | 0:83acd45a2405 | 121 | } |
manz | 0:83acd45a2405 | 122 | // go left |
manz | 0:83acd45a2405 | 123 | else if(x == 4 && y == 0 && x_dir == 0 && y_dir == 1){ |
manz | 0:83acd45a2405 | 124 | serial.printf("Go left \r\n"); |
manz | 0:83acd45a2405 | 125 | } |
manz | 0:83acd45a2405 | 126 | // go right |
manz | 0:83acd45a2405 | 127 | else if(x == 4 && y == 0){ |
manz | 0:83acd45a2405 | 128 | serial.printf("Go right \r\n"); |
manz | 0:83acd45a2405 | 129 | } |
manz | 0:83acd45a2405 | 130 | else { |
manz | 0:83acd45a2405 | 131 | serial.printf("Coordinates \r\n"); |
manz | 0:83acd45a2405 | 132 | } |
manz | 0:83acd45a2405 | 133 | |
manz | 0:83acd45a2405 | 134 | // putting values into array |
manz | 0:83acd45a2405 | 135 | serial.printf("try to put coordinates \r\n"); |
manz | 0:83acd45a2405 | 136 | one_slot.wait(); |
manz | 0:83acd45a2405 | 137 | if (counter == size){ |
manz | 0:83acd45a2405 | 138 | counter = 0; |
manz | 0:83acd45a2405 | 139 | } |
manz | 0:83acd45a2405 | 140 | path_x[counter] = x; |
manz | 0:83acd45a2405 | 141 | path_y[counter] = y; |
manz | 0:83acd45a2405 | 142 | counter++; |
manz | 0:83acd45a2405 | 143 | one_slot.release(); |
manz | 0:83acd45a2405 | 144 | serial.printf("put coordinates \r\n"); |
manz | 0:83acd45a2405 | 145 | |
manz | 0:83acd45a2405 | 146 | Thread::wait(100); |
manz | 0:83acd45a2405 | 147 | } |
manz | 0:83acd45a2405 | 148 | |
manz | 0:83acd45a2405 | 149 | if(serial.readable()) |
manz | 0:83acd45a2405 | 150 | { |
manz | 0:83acd45a2405 | 151 | if(!count_on) |
manz | 0:83acd45a2405 | 152 | { |
manz | 0:83acd45a2405 | 153 | count_on = 1; |
manz | 0:83acd45a2405 | 154 | } |
manz | 0:83acd45a2405 | 155 | interval = 0; |
manz | 0:83acd45a2405 | 156 | txbuf[txlen] = serial.getc(); |
manz | 0:83acd45a2405 | 157 | txlen++; |
manz | 0:83acd45a2405 | 158 | } |
manz | 0:83acd45a2405 | 159 | |
manz | 0:83acd45a2405 | 160 | if(count_on) // Count the interval after receiving a new char from terminate |
manz | 0:83acd45a2405 | 161 | { |
manz | 0:83acd45a2405 | 162 | interval++; |
manz | 0:83acd45a2405 | 163 | } |
manz | 0:83acd45a2405 | 164 | |
manz | 0:83acd45a2405 | 165 | if(interval == 10) // If there is no char available last the interval, send the received chars to central. |
manz | 0:83acd45a2405 | 166 | { |
manz | 0:83acd45a2405 | 167 | interval = 0; |
manz | 0:83acd45a2405 | 168 | count_on = 0; |
manz | 0:83acd45a2405 | 169 | serial.printf("Received from terminal: %d bytes\r\n", txlen); |
manz | 0:83acd45a2405 | 170 | txCharacteristic.setValue((const unsigned char *)txbuf, txlen); |
manz | 0:83acd45a2405 | 171 | txlen = 0; |
manz | 0:83acd45a2405 | 172 | } |
manz | 0:83acd45a2405 | 173 | } |
manz | 0:83acd45a2405 | 174 | // central disconnected |
manz | 0:83acd45a2405 | 175 | serial.printf("Disconnected from central\r\n"); |
manz | 0:83acd45a2405 | 176 | } |
manz | 0:83acd45a2405 | 177 | } |
manz | 0:83acd45a2405 | 178 | } |
manz | 0:83acd45a2405 | 179 | |
manz | 0:83acd45a2405 | 180 | |
manz | 0:83acd45a2405 | 181 | void move_thread(void const *args){ |
manz | 0:83acd45a2405 | 182 | int angle; |
manz | 0:83acd45a2405 | 183 | |
manz | 0:83acd45a2405 | 184 | Timer t; |
manz | 0:83acd45a2405 | 185 | t.start(); |
manz | 0:83acd45a2405 | 186 | |
manz | 0:83acd45a2405 | 187 | localization L; |
manz | 0:83acd45a2405 | 188 | L.reset(); |
manz | 0:83acd45a2405 | 189 | //initally put pen down |
manz | 0:83acd45a2405 | 190 | //L.servo(0); |
manz | 0:83acd45a2405 | 191 | |
manz | 0:83acd45a2405 | 192 | float currentAngle; |
manz | 0:83acd45a2405 | 193 | float targetAngle; |
manz | 0:83acd45a2405 | 194 | float startAngle; |
manz | 0:83acd45a2405 | 195 | float diffAngle; |
manz | 0:83acd45a2405 | 196 | |
manz | 0:83acd45a2405 | 197 | int control; |
manz | 0:83acd45a2405 | 198 | |
manz | 0:83acd45a2405 | 199 | double dot,a2,b2,c2; |
manz | 0:83acd45a2405 | 200 | double x_tar,y_tar; |
manz | 0:83acd45a2405 | 201 | double cosg,gamma,distance,wait_t; |
manz | 0:83acd45a2405 | 202 | double x_taro = 0; |
manz | 0:83acd45a2405 | 203 | double y_taro = 0; |
manz | 0:83acd45a2405 | 204 | serial.printf("Started move thread\r\n"); |
manz | 0:83acd45a2405 | 205 | int newval = 0; |
manz | 0:83acd45a2405 | 206 | |
manz | 0:83acd45a2405 | 207 | while(1){ |
manz | 0:83acd45a2405 | 208 | one_slot.wait(); |
manz | 0:83acd45a2405 | 209 | if(counter != start){ |
manz | 0:83acd45a2405 | 210 | if (start == size){ |
manz | 0:83acd45a2405 | 211 | start = 0; |
manz | 0:83acd45a2405 | 212 | } |
manz | 0:83acd45a2405 | 213 | x_tar = path_x[start]; |
manz | 0:83acd45a2405 | 214 | y_tar = path_y[start]; |
manz | 0:83acd45a2405 | 215 | start++; |
manz | 0:83acd45a2405 | 216 | newval = 1; |
manz | 0:83acd45a2405 | 217 | } |
manz | 0:83acd45a2405 | 218 | one_slot.release(); |
manz | 0:83acd45a2405 | 219 | |
manz | 0:83acd45a2405 | 220 | if(newval == 1){ |
manz | 0:83acd45a2405 | 221 | serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); |
manz | 0:83acd45a2405 | 222 | newval = 0; |
manz | 0:83acd45a2405 | 223 | |
manz | 0:83acd45a2405 | 224 | //compute angle and turn direction |
manz | 0:83acd45a2405 | 225 | a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent); |
manz | 0:83acd45a2405 | 226 | b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen); |
manz | 0:83acd45a2405 | 227 | c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent); |
manz | 0:83acd45a2405 | 228 | cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2)); |
manz | 0:83acd45a2405 | 229 | gamma = acos(cosg)/3.14159*180; |
manz | 0:83acd45a2405 | 230 | |
manz | 0:83acd45a2405 | 231 | dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); |
manz | 0:83acd45a2405 | 232 | |
manz | 0:83acd45a2405 | 233 | serial.printf("Angle: %f \r\n",gamma); |
manz | 0:83acd45a2405 | 234 | angle = ceil(180 - gamma); |
manz | 0:83acd45a2405 | 235 | serial.printf("Turning angle: %i \r\n",angle); |
manz | 0:83acd45a2405 | 236 | |
manz | 0:83acd45a2405 | 237 | //put pen up |
manz | 0:83acd45a2405 | 238 | //L.servo(30); |
manz | 0:83acd45a2405 | 239 | |
manz | 0:83acd45a2405 | 240 | currentAngle = L.getAngle(); |
manz | 0:83acd45a2405 | 241 | if(dot > 0){ |
manz | 0:83acd45a2405 | 242 | //serial.printf("Turn right \r\n"); |
manz | 0:83acd45a2405 | 243 | targetAngle = fmod(currentAngle+angle,360); |
manz | 0:83acd45a2405 | 244 | |
manz | 0:83acd45a2405 | 245 | while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ |
manz | 0:83acd45a2405 | 246 | control = step_right(); |
manz | 0:83acd45a2405 | 247 | serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); |
manz | 0:83acd45a2405 | 248 | } |
manz | 0:83acd45a2405 | 249 | //serial.printf("Reached target \r\n"); |
manz | 0:83acd45a2405 | 250 | } |
manz | 0:83acd45a2405 | 251 | else if(dot < 0){ |
manz | 0:83acd45a2405 | 252 | //serial.printf("Turn left \r\n"); |
manz | 0:83acd45a2405 | 253 | targetAngle = fmod(currentAngle-angle,360); |
manz | 0:83acd45a2405 | 254 | while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ |
manz | 0:83acd45a2405 | 255 | control = step_left(); |
manz | 0:83acd45a2405 | 256 | serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); |
manz | 0:83acd45a2405 | 257 | } |
manz | 0:83acd45a2405 | 258 | } |
manz | 0:83acd45a2405 | 259 | else{ |
manz | 0:83acd45a2405 | 260 | } |
manz | 0:83acd45a2405 | 261 | //put pen down again |
manz | 0:83acd45a2405 | 262 | //L.servo(0); |
manz | 0:83acd45a2405 | 263 | |
manz | 0:83acd45a2405 | 264 | startAngle = L.getAngle(); |
manz | 0:83acd45a2405 | 265 | serial.printf("Current angle: %f \r\n",startAngle); |
manz | 0:83acd45a2405 | 266 | //compute length of path til target |
manz | 0:83acd45a2405 | 267 | distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen)); |
manz | 0:83acd45a2405 | 268 | |
manz | 0:83acd45a2405 | 269 | //forward SCRIBE til target |
manz | 0:83acd45a2405 | 270 | wait_t = distance/speed*t_factor; |
manz | 0:83acd45a2405 | 271 | t.reset(); |
manz | 0:83acd45a2405 | 272 | while(t.read() < wait_t){ |
manz | 0:83acd45a2405 | 273 | control = step_f(); |
manz | 0:83acd45a2405 | 274 | } |
manz | 0:83acd45a2405 | 275 | serial.printf("Reached destination \r\n"); |
manz | 0:83acd45a2405 | 276 | |
manz | 0:83acd45a2405 | 277 | |
manz | 0:83acd45a2405 | 278 | //update pen and center when at target |
manz | 0:83acd45a2405 | 279 | x_pen = x_tar; |
manz | 0:83acd45a2405 | 280 | y_pen = y_tar; |
manz | 0:83acd45a2405 | 281 | serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen); |
manz | 0:83acd45a2405 | 282 | x_cent = x_taro; |
manz | 0:83acd45a2405 | 283 | y_cent = y_taro; |
manz | 0:83acd45a2405 | 284 | serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); |
manz | 0:83acd45a2405 | 285 | x_taro = x_tar; |
manz | 0:83acd45a2405 | 286 | y_taro = y_tar; |
manz | 0:83acd45a2405 | 287 | |
manz | 0:83acd45a2405 | 288 | } |
manz | 0:83acd45a2405 | 289 | Thread::wait(100); |
manz | 0:83acd45a2405 | 290 | } |
manz | 0:83acd45a2405 | 291 | } |
manz | 0:83acd45a2405 | 292 | |
manz | 0:83acd45a2405 | 293 | int main() |
manz | 0:83acd45a2405 | 294 | { |
manz | 0:83acd45a2405 | 295 | serial.printf("Starting the threads"); |
manz | 0:83acd45a2405 | 296 | |
manz | 0:83acd45a2405 | 297 | Thread bluetooth(bluetooth_thread); |
manz | 0:83acd45a2405 | 298 | Thread move(move_thread); |
manz | 0:83acd45a2405 | 299 | |
manz | 0:83acd45a2405 | 300 | Thread::wait(osWaitForever); |
manz | 0:83acd45a2405 | 301 | |
manz | 0:83acd45a2405 | 302 | } |