Diff: main.cpp

Revision:

5:1da4d4050306

Parent:

3:63aef644e6d2

Child:

7:1bb3b5b66fe8

--- 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