Diff: main.cpp

Revision:

3:63aef644e6d2

Parent:

0:83acd45a2405

Child:

5:1da4d4050306

--- a/main.cpp	Fri Apr 22 18:30:57 2016 +0000
+++ b/main.cpp	Sat Apr 23 02:46:30 2016 +0000
@@ -32,12 +32,18 @@
 unsigned char count_on = 0;
 
 // array to save the values of the user
-const int size = 40;
+const int size = 400;
 int path_x [size];
 int path_y [size];
 int counter = 0;
 int start = 0;
 
+const int shape_size = 10;
+int shape_x [shape_size];
+int shape_y [shape_size];
+
+int mode = -1;
+
 //coordinates of the pen and the centre of SCRIBE at beginning
 double x_pen = 0;
 double y_pen = 0;
@@ -51,6 +57,7 @@
 
 Semaphore one_slot(1);
 
+localization L;
 
 void bluetooth_thread(void const *args){
     serial.printf("Serial begin!\r\n");
@@ -74,6 +81,8 @@
     //return value for move functions
     int ret;
     
+    int width, length;
+    
     serial.printf("BLE UART Peripheral begin!\r\n");
     
     while(1)
@@ -92,57 +101,53 @@
                 {
                     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)
                     {
                         serial.printf("%d, ",val[i++]);
                     }
+                    
                     serial.printf("\r\n");
-                    x = (int) val[0];
-                    y = (int) val[1];
-                    x_dir = (int) val[2];
-                    y_dir = (int) val[3];
-                    
-                    // see if values are negative
-                    if(x_dir == 1){
-                        x = -1*x;
-                    }
-                    if(y_dir == 1){
-                        y = -1*y;
-                    }
-                    // go forward
-                    if(x == 0 && y == 4){
-                        serial.printf("Go forward \r\n"); 
-                    }
-                    // go backward
-                    else if(x == 0 && y == -4){
-                        serial.printf("Go backward \r\n"); 
+                    //determine mode of signal
+                    if(rxlen == 1){
+                        if (mode != 13){
+                            mode = (int) val[0];
+                        } 
                     }
-                    // go left
-                    else if(x == 4 && y == 0 && x_dir == 0 && y_dir == 1){
-                        serial.printf("Go left \r\n"); 
-                    }
-                    // go right
-                    else if(x == 4 && y == 0){
-                        serial.printf("Go right \r\n"); 
-                    }
-                    else {
-                        serial.printf("Coordinates \r\n");
+                    if(rxlen == 4){
+                        //check if drawing mode
+                        if(mode != 4){
+                            mode = 9;
+                        }    
+                        x = (int) val[0];
+                        y = (int) val[1];
+                        x_dir = (int) val[2];
+                        y_dir = (int) 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");
+                        one_slot.wait();
+                        if (counter == size){
+                            counter = 0;
+                        }
+                        path_x[counter] = x;
+                        path_y[counter] = y;
+                        counter++;
+                        one_slot.release();
+                        serial.printf("put coordinates \r\n");
                     }
-                    
-                    // putting values into array
-                    serial.printf("try to put coordinates \r\n");
-                    one_slot.wait();
-                    if (counter == size){
-                        counter = 0;
-                    }
-                    path_x[counter] = x;
-                    path_y[counter] = y;
-                    counter++;
-                    one_slot.release();
-                    serial.printf("put coordinates \r\n");
-                    
                     Thread::wait(100);
                 }
                  
@@ -177,14 +182,25 @@
     }
 }
 
+// incomplete
+int draw_circle(double radius){
+    float currAngle = L.getAngle();
+    if(radius >= 5){ 
+        int outer = (int) radius*0.6;
+        int inner = (int) radius*0.2;
+    }
+}
+
  
 void move_thread(void const *args){
     int angle;
+    double length = 8;
+    double width = 8;
+    int radius = 5;
 
     Timer t;
     t.start();
-    
-    localization L;
+
     L.reset();
     //initally put pen down
     //L.servo(0);
@@ -195,9 +211,13 @@
     float diffAngle;
     
     int control;
+    int shape_count = 0;
+    
+    double draw_corr;
     
     double dot,a2,b2,c2;
     double x_tar,y_tar;
+    double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr;
     double cosg,gamma,distance,wait_t;
     double x_taro = 0;
     double y_taro = 0;
@@ -205,18 +225,105 @@
     int newval = 0;
     
     while(1){
-        one_slot.wait();
-        if(counter != start){
-            if (start == size){
-                start = 0;
+        // check what mode is present
+        //rectangle/square
+        if(mode == 0){
+            serial.printf("Draw rectangle \r\n");
+            //save old values and set initial
+            x_pen_pr = x_pen;
+            y_pen_pr = y_pen;
+            x_cent_pr = x_cent;
+            y_cent_pr = y_cent; 
+            x_pen = 0;
+            y_pen = 0;
+            x_cent = 0;
+            y_cent = -1;
+            
+            //set values
+            shape_x[3] = 0;
+            shape_y[3] = length;
+            shape_x[2] = width;
+            shape_y[2] = length;
+            shape_x[1] = width;
+            shape_y[1] = 0;
+            shape_x[0] = 0;
+            shape_y[0] = 0;
+            shape_count = 4;
+            
+            mode = 13;
+        }    
+        else if(mode == 1){
+            serial.printf("Draw circle \r\n");
+            //call circle function
+            control = draw_circle(radius);
+            
+            mode = -1;
+        }     
+        else if(mode == 2){
+            serial.printf("Draw triangle \r\n");
+            //save old values and set initial
+            x_pen_pr = x_pen;
+            y_pen_pr = y_pen;
+            x_cent_pr = x_cent;
+            y_cent_pr = y_cent; 
+            x_pen = 0;
+            y_pen = 0;
+            x_cent = 0;
+            y_cent = -1;
+            
+            //set values
+            shape_x[2] = 0;
+            shape_y[2] = length;
+            shape_x[1] = width;
+            shape_y[1] = 0;
+            shape_x[0] = 0;
+            shape_y[0] = 0;
+            
+            shape_count = 3;
+            mode = 13;
+        }
+        else if(mode == 3){
+            serial.printf("Reset \r\n");
+            //set initial
+            x_pen = 0;
+            y_pen = 0;
+            x_cent = 0;
+            y_cent = -1;
+            
+            mode = -1;
+        }
+        else if (mode == 4){
+            serial.printf("Draw freely \r\n");
+            draw_corr = 0.2;
+        }
+        else if (mode == 9){
+            serial.printf("Draw coordinates \r\n");
+            draw_corr = 1;
+            
+            mode = -1;
+        }
+        
+        // next coordinate is free drawing or coordinates
+        if(shape_count == 0){
+            one_slot.wait();
+            if(counter != start){
+                if (start == size){
+                    start = 0;
+                }
+                x_tar = path_x[start];
+                y_tar = path_y[start];
+                start++;
+                newval = 1; 
             }
-            x_tar = path_x[start];
-            y_tar = path_y[start];
-            start++;
-            newval = 1; 
+            one_slot.release();
         }
-        one_slot.release();
-        
+        // next coordinate is shape
+        else{
+            shape_count = shape_count - 1;
+            x_tar = shape_x[shape_count];
+            y_tar = shape_y[shape_count];
+            newval = 1;
+        }
         if(newval == 1){
             serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar);
             newval = 0;