File content as of revision 3:63aef644e6d2:

// 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 = 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;
double x_cent = 0;
double y_cent = -1;

//speed of SCRIBE
float speed = 0.3;
float t_factor = 0.15;


Semaphore one_slot(1);

localization L;

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;
    
    int width, length;
    
    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");
                    //determine mode of signal
                    if(rxlen == 1){
                        if (mode != 13){
                            mode = (int) val[0];
                        } 
                    }
                    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");
                    }
                    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");
        }
    }
}

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

    L.reset();
    //initally put pen down
    //L.servo(0);
    
    float currentAngle;
    float targetAngle;
    float startAngle;
    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;
    serial.printf("Started move thread\r\n");
    int newval = 0;
    
    while(1){
        // 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; 
            }
            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;
            
            //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);    
    
}