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:
- 7:1bb3b5b66fe8
- Parent:
- 5:1da4d4050306
- Child:
- 8:4d7b2dbdb694
--- a/main.cpp Wed May 04 23:32:45 2016 +0000 +++ b/main.cpp Wed May 04 23:58:00 2016 +0000 @@ -6,10 +6,20 @@ #include "mbed.h" #include "rtos.h" #include "localization.h" -#include "stepper.h" +#include "servo.h" Serial serial(USBTX, USBRX); - + +//Analog In for ADC +AnalogIn ain(A0); + +//Digital LED +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); + + SPI spi(p11, p12, p13); DigitalInOut BLE_RDY(p21); DigitalInOut BLE_REQ(p22); @@ -34,16 +44,17 @@ int which_thread = 0; // array to save the values of the user -const int size = 100; +const int size = 500; int path_x [size]; int path_y [size]; int path_p [size]; int counter = 0; int start = 0; + const int shape_size = 10; -int shape_x [shape_size]; -int shape_y [shape_size]; +double shape_x [shape_size]; +double shape_y [shape_size]; int mode = -1; //default mode is idle int pmode = 1; //default pen is down @@ -76,6 +87,7 @@ } int x,y,x_dir,y_dir; + int stroke = 0; /*----- BLE Utility ---------------------------------------------*/ // set advertised local name and service UUID @@ -97,14 +109,15 @@ while(1) { BLECentral central = blePeripheral.central(); - + if (central) { // central connected to peripheral serial.printf("Connected to central\r\n"); while (central.connected()) { - Thread::signal_wait(0x1); + //serial.printf("*** Connected to bluetooth \r\n"); + Thread::wait(1); // central still connected to peripheral if (rxCharacteristic.written()) { @@ -125,12 +138,14 @@ if (mode != 13){ mode = (int) val[0]; if(mode != 4 && mode !=6){ + serial.printf("Mode 1-val: %i\r\n",mode); + osSignalClear(bluetooth_id,0x1); which_thread = 1; - osSignalClear(bluetooth_id,0x1); } } } if (mode == 6){ + stroke = 0; serial.printf("Storing drawn coordinates\r\n"); serial.printf("x_coord = ["); for (int i = 0; i < size; i++) { @@ -145,13 +160,14 @@ } // stroke of pen finished if (mode == 7){ + stroke = 1; 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){ + if(mode != 4 && stroke == 0){ mode = 9; } // in draw mode - convert coordinates @@ -169,13 +185,15 @@ path_y[counter] = y; //check if pen needs to be up or down - path_p[counter] = pmode; + path_p[counter] = (int) val[4]; + serial.printf("Pen mode: %i \r\n",path_p[counter]); counter++; one_slot.release(); if(mode == 9){ + serial.printf("coordinate mode"); + osSignalClear(bluetooth_id,0x1); which_thread = 1; - osSignalClear(bluetooth_id,0x1); } //serial.printf("put coordinates \r\n"); } @@ -223,6 +241,62 @@ } } +void deadreckoning(){ + led2 = 1; + double sum; + int counter = 10; + int i; + + //calibration + sum = 0; +/* for (i = 0; i < 100; i++){ + sum += ain.read(); + } + double calibration = sum/100; + serial.printf("Calibration: %f\r\n",calibration);*/ + double threshold = 0.9; + + while(1){ + sum = 0; + step_f(); + for (i = 0; i < counter; i++){ + sum += ain.read(); + } + if(sum/counter > threshold){ + led3 = 1; + break; + } + serial.printf("Read Values: %f\r\n",sum/counter); + } + serial.printf("Turn left now \r\n"); + + //left by 90 degrees + int control; + double currentAngle = L.getAngle(); + double targetAngle = fmod(currentAngle+90,360); + while(fmod(abs(L.getAngle() - targetAngle),360)> 3){ + control = step_right(); + } + + while(1){ + sum = 0; + step_f(); + for (i = 0; i < counter; i++){ + sum += ain.read(); + } + if(sum/counter > threshold){ + led3 = 0; + break; + } + serial.printf("Read Values: %f\r\n",sum/counter); + } + serial.printf("Deadreckoning finished \r\n"); + L.reset(); +} + + + + void move_thread(void const *args){ @@ -232,11 +306,8 @@ int radius = 5; Timer t; - t.start(); - - L.reset(); //initally put pen down - //L.servo(0); + servo_reset(); float currentAngle; float targetAngle; @@ -246,15 +317,17 @@ int steps; int control; + int restore = 0; int shape_count = 0; int p_mode; int first = 0; double draw_corr = 5; + double time_factor = 1; double dot,a2,b2,c2; double x_tar,y_tar; - double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr; + double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr, x_taro_pr, y_taro_pr; double cosg,gamma,distance,wait_t; double x_taro = 0; double y_taro = 0; @@ -264,7 +337,10 @@ while(1){ // check what mode is present serial.printf("Cylce \r\n"); + serial.printf("mode: %i \r\n",mode); + Thread::signal_wait(0x1); + if(mode == -1){ serial.printf("here ends"); osSignalClear(mover_id,0x1); @@ -277,7 +353,9 @@ x_pen_pr = x_pen; y_pen_pr = y_pen; x_cent_pr = x_cent; - y_cent_pr = y_cent; + y_cent_pr = y_cent; + x_taro_pr = x_taro; + y_taro_pr = y_taro; x_pen = 0; y_pen = 0; x_cent = 0; @@ -286,15 +364,17 @@ y_taro = 0; //set values - shape_x[3] = 0; + shape_x[4] = 0; + shape_y[4] = length; + shape_x[3] = width; shape_y[3] = length; shape_x[2] = width; - shape_y[2] = length; - shape_x[1] = width; + shape_y[2] = 0; + shape_x[1] = 0; shape_y[1] = 0; shape_x[0] = 0; - shape_y[0] = 0; - shape_count = 4; + shape_y[0] = 0.1; + shape_count = 5; mode = 13; } @@ -311,7 +391,9 @@ x_pen_pr = x_pen; y_pen_pr = y_pen; x_cent_pr = x_cent; - y_cent_pr = y_cent; + y_cent_pr = y_cent; + x_taro_pr = x_taro; + y_taro_pr = y_taro; x_pen = 0; y_pen = 0; x_cent = 0; @@ -320,14 +402,16 @@ y_taro = 0; //set values - shape_x[2] = 0; - shape_y[2] = length; - shape_x[1] = width; + shape_x[3] = 0; + shape_y[3] = length; + shape_x[2] = width; + shape_y[2] = 0; + shape_x[1] = 0; shape_y[1] = 0; shape_x[0] = 0; - shape_y[0] = 0; + shape_y[0] = 0.1; - shape_count = 3; + shape_count = 4; mode = 13; } else if(mode == 3){ @@ -337,6 +421,13 @@ y_pen = 0; x_cent = 0; y_cent = -1; + x_taro = 0; + y_taro = 0; + + one_slot.wait(); + counter = 0; + start = 0; + one_slot.release(); mode = -1; } @@ -346,7 +437,6 @@ draw_corr = 1; } first = 1; - } else if (mode == 9){ serial.printf("Draw coordinates \r\n"); @@ -365,7 +455,9 @@ y_tar = (double) path_y[start]*draw_corr; p_mode = path_p[start]; start++; - if(start == counter) mode = -1; + if(start == counter){ + mode = -1; + } newval = 1; } one_slot.release(); @@ -373,83 +465,97 @@ // next coordinate is shape else{ shape_count = shape_count - 1; - x_tar = (double) shape_x[shape_count]; - y_tar = (double) shape_y[shape_count]; + x_tar = shape_x[shape_count]; + y_tar = shape_y[shape_count]; p_mode = 1; newval = 1; //last move -> unblock input if(shape_count == 0){ + restore = 1; mode = -1; } } 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 down - if (p_mode == 1){ - //serial.printf("Pen down \r\n"); - L.servo(100); - } - //put pen up - else if (p_mode == 0){ - //serial.printf("Pen up \r\n"); - L.servo(90); + // same position -> do nothing + if(x_tar == x_pen && y_tar == y_pen){ + } - - 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",fmod(abs(L.getAngle() - targetAngle),360)); + else{ + //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 down + if (p_mode == 1){ + serial.printf("-- Pen Down\r\n"); + L.servo(108); } - //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",fmod(abs(L.getAngle() - targetAngle),360)); + //put pen up + else if (p_mode == 0){ + L.servo(90); + serial.printf("-- Pen Up\r\n"); + } + + currentAngle = L.getAngle(); + if(dot > 0){ + //serial.printf("Turn right \r\n"); + targetAngle = fmod(currentAngle+angle,360); + + servo_right(); + while(fmod(abs(L.getAngle() - targetAngle),360)> 3); + servo_stop(); } - } - //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 - steps = (int) distance; - for(int i = 0; i< steps; i++){ - control = step_f(); - } - //serial.printf("Reached destination \r\n"); - - //update pen and center when at target - x_pen = x_tar; - y_pen = y_tar; + else if(dot < 0){ + //serial.printf("Turn left \r\n"); + targetAngle = fmod(currentAngle-angle,360); + servo_left(); + while(fmod(abs(L.getAngle() - targetAngle),360)> 3); + servo_stop(); + } + //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 + serial.printf("Distance to be taken: %f\r\n",distance); + + servo_f(); + wait(distance*time_factor); + servo_stop(); + //serial.printf("Reached destination \r\n"); + //update pen and center when at target + if(restore == 1){ + serial.printf("Restore previous positions \r\n"); + x_pen = x_pen_pr; + y_pen = y_pen_pr; + x_cent = x_cent_pr; + y_cent = y_cent_pr; + x_taro = x_taro_pr; + y_taro = y_taro_pr; + restore = 0; + } + else{ + x_pen = x_tar; + y_pen = y_tar; + x_cent = x_taro; + y_cent = y_taro; + x_taro = x_tar; + y_taro = 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; - + serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); } } } @@ -457,8 +563,13 @@ int main() -{ - serial.printf("Starting the threads"); +{ + L.reset(); + //L.servo(90); + wait(1); + serial.printf("Starting Dead-Reckoning\r\n"); + //deadreckoning(); + serial.printf("Starting the threads\r\n"); Thread bluetooth(bluetooth_thread); Thread move(move_thread); @@ -466,6 +577,7 @@ bluetooth_id = bluetooth.gettid(); while(1){ + Thread::wait(3); if(which_thread == 0) bluetooth.signal_set(0x1); else move.signal_set(0x1); }