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:
- 5:1da4d4050306
- Parent:
- 3:63aef644e6d2
- Child:
- 7:1bb3b5b66fe8
diff -r 63aef644e6d2 -r 1da4d4050306 main.cpp --- a/main.cpp Sat Apr 23 02:46:30 2016 +0000 +++ b/main.cpp Mon Apr 25 05:38:48 2016 +0000 @@ -4,9 +4,9 @@ #include "Arduino.h" #include "BLEPeripheral.h" #include "mbed.h" +#include "rtos.h" +#include "localization.h" #include "stepper.h" -#include "rtos.h" -#include "localization.h"; Serial serial(USBTX, USBRX); @@ -31,10 +31,13 @@ unsigned int interval = 0; unsigned char count_on = 0; +int which_thread = 0; + // array to save the values of the user -const int size = 400; +const int size = 100; int path_x [size]; int path_y [size]; +int path_p [size]; int counter = 0; int start = 0; @@ -42,7 +45,8 @@ int shape_x [shape_size]; int shape_y [shape_size]; -int mode = -1; +int mode = -1; //default mode is idle +int pmode = 1; //default pen is down //coordinates of the pen and the centre of SCRIBE at beginning double x_pen = 0; @@ -59,8 +63,18 @@ localization L; +osThreadId bluetooth_id, mover_id; + void bluetooth_thread(void const *args){ + bluetooth_id = Thread::gettid(); + serial.printf("Serial begin!\r\n"); + + FILE *fp = fopen("/local/trial.m", "w"); + if (fp == NULL) { + serial.printf("ERROR: File open!\r\n"); + } + int x,y,x_dir,y_dir; /*----- BLE Utility ---------------------------------------------*/ @@ -78,11 +92,6 @@ blePeripheral.begin(); /*---------------------------------------------------------------*/ - //return value for move functions - int ret; - - int width, length; - serial.printf("BLE UART Peripheral begin!\r\n"); while(1) @@ -93,15 +102,15 @@ { // central connected to peripheral serial.printf("Connected to central\r\n"); - while (central.connected()) - { + { + Thread::signal_wait(0x1); // 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); + /*serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); unsigned char i = 0; while(i<rxlen) @@ -109,46 +118,67 @@ serial.printf("%d, ",val[i++]); } - serial.printf("\r\n"); + serial.printf("\r\n");*/ //determine mode of signal if(rxlen == 1){ + // inputs blocked until shape finished if (mode != 13){ mode = (int) val[0]; - } + if(mode != 4 && mode !=6){ + which_thread = 1; + osSignalClear(bluetooth_id,0x1); + } + } } - if(rxlen == 4){ - //check if drawing mode - if(mode != 4){ + if (mode == 6){ + serial.printf("Storing drawn coordinates\r\n"); + serial.printf("x_coord = ["); + for (int i = 0; i < size; i++) { + serial.printf("%i ", path_x[i]); + } + serial.printf("];\n"); + serial.printf("y_coord = ["); + for (int i = 0; i < size; i++) { + serial.printf("%i ", path_y[i]); + } + serial.printf("];\n"); + } + // stroke of pen finished + if (mode == 7){ + serial.printf("Other threads can proceed \r\n"); + osSignalClear(bluetooth_id,0x1); + which_thread = 1; + } + if(rxlen == 5){ + // in coordinates mode - accept negative coordinates + if(mode == 6){ mode = 9; - } - x = (int) val[0]; - y = (int) val[1]; - x_dir = (int) val[2]; - y_dir = (int) val[3]; + } + // in draw mode - convert coordinates + x = (int) val[0]*256 + val[1]; + y = (int) val[2]*256 + val[3]; - // see if values are negative - if(x_dir == 1){ - x = -1*x; - } - if(y_dir == 1){ - y = -1*y; - } - if (rxlen == 1){ - mode = (int) val[0]; - } // putting values into array - serial.printf("try to put coordinates \r\n"); + //serial.printf("try to put coordinates \r\n"); one_slot.wait(); if (counter == size){ + serial.printf("Overwriting values \r\n"); counter = 0; } path_x[counter] = x; path_y[counter] = y; + + //check if pen needs to be up or down + path_p[counter] = pmode; + counter++; one_slot.release(); - serial.printf("put coordinates \r\n"); + if(mode == 9){ + which_thread = 1; + osSignalClear(bluetooth_id,0x1); + } + //serial.printf("put coordinates \r\n"); } - Thread::wait(100); } if(serial.readable()) @@ -171,17 +201,19 @@ { interval = 0; count_on = 0; - serial.printf("Received from terminal: %d bytes\r\n", txlen); + //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"); } } } + // incomplete int draw_circle(double radius){ float currAngle = L.getAngle(); @@ -193,9 +225,10 @@ void move_thread(void const *args){ + int angle; - double length = 8; - double width = 8; + double length = 40; + double width = 40; int radius = 5; Timer t; @@ -210,10 +243,14 @@ float startAngle; float diffAngle; + int steps; + int control; int shape_count = 0; + int p_mode; + int first = 0; - double draw_corr; + double draw_corr = 5; double dot,a2,b2,c2; double x_tar,y_tar; @@ -223,11 +260,18 @@ double y_taro = 0; serial.printf("Started move thread\r\n"); int newval = 0; - + while(1){ // check what mode is present + serial.printf("Cylce \r\n"); + Thread::signal_wait(0x1); + if(mode == -1){ + serial.printf("here ends"); + osSignalClear(mover_id,0x1); + which_thread = 0; + } //rectangle/square - if(mode == 0){ + else if(mode == 0){ serial.printf("Draw rectangle \r\n"); //save old values and set initial x_pen_pr = x_pen; @@ -238,6 +282,8 @@ y_pen = 0; x_cent = 0; y_cent = -1; + x_taro = 0; + y_taro = 0; //set values shape_x[3] = 0; @@ -270,6 +316,8 @@ y_pen = 0; x_cent = 0; y_cent = -1; + x_taro = 0; + y_taro = 0; //set values shape_x[2] = 0; @@ -292,14 +340,17 @@ mode = -1; } - else if (mode == 4){ - serial.printf("Draw freely \r\n"); - draw_corr = 0.2; + else if (mode == 7){ + if (first == 0){ + serial.printf("Draw freely \r\n"); + draw_corr = 1; + } + first = 1; + } else if (mode == 9){ serial.printf("Draw coordinates \r\n"); - draw_corr = 1; - + draw_corr = 5; mode = -1; } @@ -310,9 +361,11 @@ if (start == size){ start = 0; } - x_tar = path_x[start]; - y_tar = path_y[start]; + x_tar = (double) path_x[start]*draw_corr; + y_tar = (double) path_y[start]*draw_corr; + p_mode = path_p[start]; start++; + if(start == counter) mode = -1; newval = 1; } one_slot.release(); @@ -320,9 +373,15 @@ // next coordinate is shape else{ shape_count = shape_count - 1; - x_tar = shape_x[shape_count]; - y_tar = shape_y[shape_count]; + x_tar = (double) shape_x[shape_count]; + y_tar = (double) shape_y[shape_count]; + p_mode = 1; newval = 1; + + //last move -> unblock input + if(shape_count == 0){ + mode = -1; + } } if(newval == 1){ serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); @@ -337,12 +396,20 @@ dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); - serial.printf("Angle: %f \r\n",gamma); + //serial.printf("Angle: %f \r\n",gamma); angle = ceil(180 - gamma); serial.printf("Turning angle: %i \r\n",angle); + //put pen down + if (p_mode == 1){ + //serial.printf("Pen down \r\n"); + L.servo(100); + } //put pen up - //L.servo(30); + else if (p_mode == 0){ + //serial.printf("Pen up \r\n"); + L.servo(90); + } currentAngle = L.getAngle(); if(dot > 0){ @@ -351,7 +418,7 @@ while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ control = step_right(); - serial.printf("Turning angle: %f \r\n",abs(L.getAngle() - targetAngle)); + //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360)); } //serial.printf("Reached target \r\n"); } @@ -360,27 +427,18 @@ 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)); + //serial.printf("Turning angle: %f \r\n",fmod(abs(L.getAngle() - targetAngle),360)); } } - 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){ + steps = (int) distance; + for(int i = 0; i< steps; i++){ control = step_f(); } - serial.printf("Reached destination \r\n"); - + //serial.printf("Reached destination \r\n"); //update pen and center when at target x_pen = x_tar; @@ -392,18 +450,23 @@ 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); + mover_id = move.gettid(); + bluetooth_id = bluetooth.gettid(); - Thread::wait(osWaitForever); - + while(1){ + if(which_thread == 0) bluetooth.signal_set(0x1); + else move.signal_set(0x1); + } } \ No newline at end of file