![](/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
Diff: main.cpp
- Revision:
- 0:83acd45a2405
- Child:
- 3:63aef644e6d2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 21 22:01:19 2016 +0000 @@ -0,0 +1,302 @@ +// main of SCRIBE stepper + +// Import libraries +#include "Arduino.h" +#include "BLEPeripheral.h" +#include "mbed.h" +#include "stepper.h" +#include "rtos.h" +#include "localization.h"; + +Serial serial(USBTX, USBRX); + +SPI spi(p11, p12, p13); +DigitalInOut BLE_RDY(p21); +DigitalInOut BLE_REQ(p22); +DigitalInOut BLE_RESET(p23); + +unsigned char txbuf[16] = {0}; +unsigned char txlen = 0; + +// create peripheral +BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET); + +// create service w/ uuid +BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e"); + +// create characteristics +BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20); +BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20); + +unsigned int interval = 0; +unsigned char count_on = 0; + +// array to save the values of the user +const int size = 40; +int path_x [size]; +int path_y [size]; +int counter = 0; +int start = 0; + +//coordinates of the pen and the centre of SCRIBE at beginning +double x_pen = 0; +double y_pen = 0; +double x_cent = 0; +double y_cent = -1; + +//speed of SCRIBE +float speed = 0.3; +float t_factor = 0.15; + + +Semaphore one_slot(1); + + +void bluetooth_thread(void const *args){ + serial.printf("Serial begin!\r\n"); + int x,y,x_dir,y_dir; + + /*----- BLE Utility ---------------------------------------------*/ + // set advertised local name and service UUID + blePeripheral.setLocalName("BLE Shield"); + + blePeripheral.setAdvertisedServiceUuid(uartService.uuid()); + + // add service and characteristic + blePeripheral.addAttribute(uartService); + blePeripheral.addAttribute(rxCharacteristic); + blePeripheral.addAttribute(txCharacteristic); + + // begin initialization + blePeripheral.begin(); + /*---------------------------------------------------------------*/ + + //return value for move functions + int ret; + + serial.printf("BLE UART Peripheral begin!\r\n"); + + while(1) + { + BLECentral central = blePeripheral.central(); + + if (central) + { + // central connected to peripheral + serial.printf("Connected to central\r\n"); + + while (central.connected()) + { + // central still connected to peripheral + if (rxCharacteristic.written()) + { + unsigned char rxlen = rxCharacteristic.valueLength(); + const unsigned char *val = rxCharacteristic.value(); + serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); + unsigned char i = 0; + while(i<rxlen) + { + serial.printf("%d, ",val[i++]); + } + serial.printf("\r\n"); + x = (int) val[0]; + y = (int) val[1]; + x_dir = (int) val[2]; + y_dir = (int) val[3]; + + // see if values are negative + if(x_dir == 1){ + x = -1*x; + } + if(y_dir == 1){ + y = -1*y; + } + // go forward + if(x == 0 && y == 4){ + serial.printf("Go forward \r\n"); + } + // go backward + else if(x == 0 && y == -4){ + serial.printf("Go backward \r\n"); + } + // go left + else if(x == 4 && y == 0 && x_dir == 0 && y_dir == 1){ + serial.printf("Go left \r\n"); + } + // go right + else if(x == 4 && y == 0){ + serial.printf("Go right \r\n"); + } + else { + serial.printf("Coordinates \r\n"); + } + + // putting values into array + serial.printf("try to put coordinates \r\n"); + one_slot.wait(); + if (counter == size){ + counter = 0; + } + path_x[counter] = x; + path_y[counter] = y; + counter++; + one_slot.release(); + serial.printf("put coordinates \r\n"); + + Thread::wait(100); + } + + if(serial.readable()) + { + if(!count_on) + { + count_on = 1; + } + interval = 0; + txbuf[txlen] = serial.getc(); + txlen++; + } + + if(count_on) // Count the interval after receiving a new char from terminate + { + interval++; + } + + if(interval == 10) // If there is no char available last the interval, send the received chars to central. + { + interval = 0; + count_on = 0; + serial.printf("Received from terminal: %d bytes\r\n", txlen); + txCharacteristic.setValue((const unsigned char *)txbuf, txlen); + txlen = 0; + } + } + // central disconnected + serial.printf("Disconnected from central\r\n"); + } + } +} + + +void move_thread(void const *args){ + int angle; + + Timer t; + t.start(); + + localization L; + L.reset(); + //initally put pen down + //L.servo(0); + + float currentAngle; + float targetAngle; + float startAngle; + float diffAngle; + + int control; + + double dot,a2,b2,c2; + double x_tar,y_tar; + double cosg,gamma,distance,wait_t; + double x_taro = 0; + double y_taro = 0; + serial.printf("Started move thread\r\n"); + int newval = 0; + + while(1){ + one_slot.wait(); + if(counter != start){ + if (start == size){ + start = 0; + } + x_tar = path_x[start]; + y_tar = path_y[start]; + start++; + newval = 1; + } + one_slot.release(); + + if(newval == 1){ + serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); + newval = 0; + + //compute angle and turn direction + a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent); + b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen); + c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent); + cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2)); + gamma = acos(cosg)/3.14159*180; + + dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); + + serial.printf("Angle: %f \r\n",gamma); + angle = ceil(180 - gamma); + serial.printf("Turning angle: %i \r\n",angle); + + //put pen up + //L.servo(30); + + currentAngle = L.getAngle(); + if(dot > 0){ + //serial.printf("Turn right \r\n"); + targetAngle = fmod(currentAngle+angle,360); + + while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ + control = step_right(); + serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); + } + //serial.printf("Reached target \r\n"); + } + else if(dot < 0){ + //serial.printf("Turn left \r\n"); + targetAngle = fmod(currentAngle-angle,360); + while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ + control = step_left(); + serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); + } + } + else{ + } + //put pen down again + //L.servo(0); + + startAngle = L.getAngle(); + serial.printf("Current angle: %f \r\n",startAngle); + //compute length of path til target + distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen)); + + //forward SCRIBE til target + wait_t = distance/speed*t_factor; + t.reset(); + while(t.read() < wait_t){ + control = step_f(); + } + serial.printf("Reached destination \r\n"); + + + //update pen and center when at target + x_pen = x_tar; + y_pen = y_tar; + serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen); + x_cent = x_taro; + y_cent = y_taro; + serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); + x_taro = x_tar; + y_taro = y_tar; + + } + Thread::wait(100); + } +} + +int main() +{ + serial.printf("Starting the threads"); + + Thread bluetooth(bluetooth_thread); + Thread move(move_thread); + + Thread::wait(osWaitForever); + +} \ No newline at end of file