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