File content as of revision 0:83acd45a2405:

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