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);
     }