SCRIBE / Mbed 2 deprecated SCRIBE_stepper

scribe robot working with stepper motors

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed

main.cpp@0:83acd45a2405, 2016-04-21 (annotated)

Committer:
manz
Date:
Thu Apr 21 22:01:19 2016 +0000
Revision:
0:83acd45a2405
Child:
2:63aef644e6d2
stepper version of SCRIBE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manz 0:83acd45a2405 1 // main of SCRIBE stepper
manz 0:83acd45a2405 2
manz 0:83acd45a2405 3 // Import libraries
manz 0:83acd45a2405 4 #include "Arduino.h"
manz 0:83acd45a2405 5 #include "BLEPeripheral.h"
manz 0:83acd45a2405 6 #include "mbed.h"
manz 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 }