Diff: main.cpp

Revision:

0:83acd45a2405

Child:

3:63aef644e6d2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 21 22:01:19 2016 +0000
@@ -0,0 +1,302 @@
+// main of SCRIBE stepper 
+
+// Import libraries 
+#include "Arduino.h"
+#include "BLEPeripheral.h"
+#include "mbed.h"
+#include "stepper.h"
+#include "rtos.h"
+#include "localization.h";
+ 
+Serial serial(USBTX, USBRX);
+ 
+SPI spi(p11, p12, p13);      
+DigitalInOut BLE_RDY(p21);  
+DigitalInOut BLE_REQ(p22);   
+DigitalInOut BLE_RESET(p23); 
+ 
+unsigned char txbuf[16] = {0};
+unsigned char txlen = 0;
+ 
+// create peripheral
+BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET);
+ 
+// create service w/ uuid
+BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e");
+ 
+// create characteristics
+BLECharacteristic    txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
+BLECharacteristic    rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
+ 
+unsigned int interval = 0;
+unsigned char count_on = 0;
+
+// array to save the values of the user
+const int size = 40;
+int path_x [size];
+int path_y [size];
+int counter = 0;
+int start = 0;
+
+//coordinates of the pen and the centre of SCRIBE at beginning
+double x_pen = 0;
+double y_pen = 0;
+double x_cent = 0;
+double y_cent = -1;
+
+//speed of SCRIBE
+float speed = 0.3;
+float t_factor = 0.15;
+
+
+Semaphore one_slot(1);
+
+
+void bluetooth_thread(void const *args){
+    serial.printf("Serial begin!\r\n");
+    int x,y,x_dir,y_dir;
+    
+    /*----- BLE Utility ---------------------------------------------*/
+    // set advertised local name and service UUID
+    blePeripheral.setLocalName("BLE Shield");
+    
+    blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
+    
+    // add service and characteristic
+    blePeripheral.addAttribute(uartService);
+    blePeripheral.addAttribute(rxCharacteristic);
+    blePeripheral.addAttribute(txCharacteristic);
+    
+    // begin initialization
+    blePeripheral.begin();
+    /*---------------------------------------------------------------*/
+    
+    //return value for move functions
+    int ret;
+    
+    serial.printf("BLE UART Peripheral begin!\r\n");
+    
+    while(1)
+    {
+        BLECentral central = blePeripheral.central();
+        
+        if (central) 
+        {
+            // central connected to peripheral
+            serial.printf("Connected to central\r\n");
+            
+            while (central.connected()) 
+            {
+                // 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); 
+                    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"); 
+                    }
+                    // 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");
+                    }
+                    
+                    // 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);
+                }
+                 
+                if(serial.readable())  
+                {
+                    if(!count_on)
+                    {
+                        count_on = 1;
+                    }
+                    interval = 0;
+                    txbuf[txlen] = serial.getc();
+                    txlen++;
+                }
+                
+                if(count_on)    // Count the interval after receiving a new char from terminate
+                {
+                    interval++;
+                }
+                
+                if(interval == 10)   // If there is no char available last the interval, send the received chars to central.
+                {
+                    interval = 0;
+                    count_on = 0;
+                    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");
+        }
+    }
+}
+
+ 
+void move_thread(void const *args){
+    int angle;
+
+    Timer t;
+    t.start();
+    
+    localization L;
+    L.reset();
+    //initally put pen down
+    //L.servo(0);
+    
+    float currentAngle;
+    float targetAngle;
+    float startAngle;
+    float diffAngle;
+    
+    int control;
+    
+    double dot,a2,b2,c2;
+    double x_tar,y_tar;
+    double cosg,gamma,distance,wait_t;
+    double x_taro = 0;
+    double y_taro = 0;
+    serial.printf("Started move thread\r\n");
+    int newval = 0;
+    
+    while(1){
+        one_slot.wait();
+        if(counter != start){
+            if (start == size){
+                start = 0;
+            }
+            x_tar = path_x[start];
+            y_tar = path_y[start];
+            start++;
+            newval = 1; 
+        }
+        one_slot.release();
+        
+        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 up
+            //L.servo(30);
+            
+            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",abs(L.getAngle() - targetAngle));
+                }
+                //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",abs(L.getAngle() - targetAngle));
+                }
+            }
+            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){
+                control = step_f();    
+            }
+            serial.printf("Reached destination \r\n");
+            
+            
+            //update pen and center when at target
+            x_pen = x_tar;
+            y_pen = 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;
+            
+        }    
+        Thread::wait(100);
+    }
+}
+
+int main()
+{ 
+    serial.printf("Starting the threads");
+    
+    Thread bluetooth(bluetooth_thread);
+    Thread move(move_thread);
+    
+    Thread::wait(osWaitForever);    
+    
+}
\ No newline at end of file