![](/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:
- 9:946d400b2f13
- Parent:
- 8:4d7b2dbdb694
- Child:
- 12:01250ea24795
--- a/main.cpp Thu May 05 01:43:03 2016 +0000 +++ b/main.cpp Thu May 05 21:52:50 2016 +0000 @@ -1,4 +1,4 @@ -// main of SCRIBE stepper +// main of SCRIBE servo // Import libraries #include "Arduino.h" @@ -10,8 +10,9 @@ Serial serial(USBTX, USBRX); -//Analog In for ADC -AnalogIn ain(A0); +//Analog In for ADC A0 -> p15 +AnalogIn ain1(A0); +AnalogIn ain2(A1); //Digital LED DigitalOut led1(LED1); @@ -241,68 +242,12 @@ } } -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){ int angle; - double length = 40; - double width = 40; + double length = 8; + double width = 8; int radius = 5; Timer t; @@ -322,7 +267,7 @@ int p_mode; int first = 0; - double draw_corr = 5; + double draw_corr = 2; double time_factor = 0.05; double dot,a2,b2,c2; @@ -333,6 +278,16 @@ double y_taro = 0; serial.printf("Started move thread\r\n"); int newval = 0; + + //values for encoding + int counter_l = 0; + int on_l; + int counter_r = 0; + int on_r; + double sum_r = 0; + double sum_l = 0; + double value_r; + double value_l; while(1){ // check what mode is present @@ -440,7 +395,7 @@ } else if (mode == 9){ serial.printf("Draw coordinates \r\n"); - draw_corr = 5; + draw_corr = 2; mode = -1; } @@ -500,11 +455,11 @@ //put pen down if (p_mode == 1){ serial.printf("-- Pen Down\r\n"); - L.servo(108); + L.servo(30); } //put pen up else if (p_mode == 0){ - L.servo(90); + L.servo(45); serial.printf("-- Pen Up\r\n"); } @@ -530,8 +485,74 @@ //forward SCRIBE til target serial.printf("Distance to be taken: %f\r\n",distance); + int counter_m = (int) rint(distance); + + // find position of wheels + for (int i = 0; i < 100; i++){ + sum_r += ain1.read(); + sum_l += ain2.read(); + } + value_r = sum_r/100; + value_l = sum_l/100; + + if(value_r < 0.93){ + on_r = 0; + } + else{ + on_r = 1; + } + if(value_l < 0.93){ + on_l = 0; + } + else{ + on_l = 1; + } + serial.printf("Initially: left: %d, right: %d\r\n",on_l,on_r); servo_f(); - wait(distance*time_factor); + //start decrement counter + counter_r = counter_m; + counter_l = counter_m; + while (counter_m > 0){ + sum_r = 0; + sum_l = 0; + for (int i = 0; i < 100; i++){ + sum_r += ain1.read(); + sum_l += ain2.read(); + } + value_r = sum_r/100; + value_l = sum_l/100; + //printf("Value: %f\r\n",value); + if(value_r < 0.93 && on_r == 1){ + printf("Value right: %f\r\n",value_r); + on_r = 0; + } + else if (value_r > 0.95 && on_r == 0){ + printf("Value right: %f\r\n",value_r); + on_r = 1; + counter_r--; + printf("*** Right Counter is: %d\r\n",counter_r); + } + if(value_l < 0.93 && on_l == 1){ + printf("Value left: %f\r\n",value_l); + on_l = 0; + } + else if (value_l > 0.95 && on_l == 0){ + printf("Value left: %f\r\n",value_l); + on_l = 1; + counter_l--; + printf("*** Left Counter is: %d\r\n",counter_l); + } + + if(counter_l >= counter_r){ + counter_m = counter_r; + } + else{ + counter_m = counter_l; + } + printf("*** Distance to take is: %d\r\n",counter_m); + } + //servo_f(); + //wait(distance*time_factor); servo_stop(); //serial.printf("Reached destination \r\n"); //update pen and center when at target