File content as of revision 5:1da4d4050306:

// main of SCRIBE stepper 

// Import libraries 
#include "Arduino.h"
#include "BLEPeripheral.h"
#include "mbed.h"
#include "rtos.h"
#include "localization.h"
#include "stepper.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;

int which_thread = 0;

// array to save the values of the user
const int size = 100;
int path_x [size];
int path_y [size];
int path_p [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; //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;
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;

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

                    unsigned char i = 0;
                    while(i<rxlen)
                    {
                        serial.printf("%d, ",val[i++]);
                    }
                    
                    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 (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;
                        }
                        // in draw mode - convert coordinates
                        x = (int) val[0]*256 + val[1];
                        y = (int) val[2]*256 + val[3];
                        
                        // putting values into array
                        //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();
                        if(mode == 9){
                            which_thread = 1;
                            osSignalClear(bluetooth_id,0x1);
                        }
                        //serial.printf("put coordinates \r\n");
                    }
                }
                 
                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 = 40;
    double width = 40;
    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 steps;
    
    int control;
    int shape_count = 0;
    int p_mode;
    int first = 0;
    
    double draw_corr = 5;
    
    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
        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
        else 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;
            x_taro = 0;
            y_taro = 0;
            
            //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;
            x_taro = 0;
            y_taro = 0;
            
            //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 == 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 = 5; 
            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 = (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();
        }
        // next coordinate is shape
        else{
            shape_count = shape_count - 1;
            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);
            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 down
            if (p_mode == 1){
                //serial.printf("Pen down \r\n");
                L.servo(100);
            }
            //put pen up
            else if (p_mode == 0){
                //serial.printf("Pen up \r\n");
                L.servo(90);
            }
            
            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",fmod(abs(L.getAngle() - targetAngle),360));
                }
                //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",fmod(abs(L.getAngle() - targetAngle),360));
                }
            }
            //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
            steps = (int) distance; 
            for(int i = 0; i< steps; i++){
                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;
            
        }   
    }
}



int main()
{ 
    serial.printf("Starting the threads");
    
    Thread bluetooth(bluetooth_thread);
    Thread move(move_thread);
    mover_id = move.gettid();
    bluetooth_id = bluetooth.gettid();
    
    while(1){
        if(which_thread == 0) bluetooth.signal_set(0x1);
        else move.signal_set(0x1);
    }    
}