File content as of revision 6:ca112c3083bb:

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

//Analog In for ADC
AnalogIn  ain(A0);

//Digital LED
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);


SPI spi(p11, p12, p13);      
DigitalInOut BLE_RDY(p21);  
DigitalInOut BLE_REQ(p22);   
DigitalInOut BLE_RESET(p23); 


// pins for IR localization
Serial pc(USBTX, USBRX); // tx, rx
I2C i2c(p9, p10);        // sda, scl
const int addr = 0xB0;   // define the I2C Address of camera
 
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 = 500;
int path_x [size];
int path_y [size];
int path_p [size];
int counter = 0;
int start = 0;


const int shape_size = 10;
double shape_x [shape_size];
double 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.8;
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;
    int stroke = 0;
    
    /*----- 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()) 
            {   
                //serial.printf("*** Connected to bluetooth \r\n");
                Thread::wait(1);
                // 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];
                            serial.printf("Mode 1-val: %i\r\n",mode);
                            if(mode != 4 && mode !=6){
                                osSignalClear(bluetooth_id,0x1);
                                which_thread = 1;
                            }
                        }
                    }
                    if (mode == 6){
                            stroke = 0;
                            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){
                        stroke = 1;
                        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 != 4 && stroke == 0){
                            serial.printf("*** Mode 1-val: %i\r\n",mode);
                            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] = (int) val[4];
                        //serial.printf("Pen mode: %i \r\n",path_p[counter]);
                        
                        counter++;
                        one_slot.release();
                        if(mode == 9){
                            serial.printf("coordinate mode");
                            osSignalClear(bluetooth_id,0x1);
                            which_thread = 1;
                        }
                        //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 i2c_write2(int addr, char a, char b)
{
    char cmd[2];
    
    cmd[0] = a;
    cmd[1] = b;
    i2c.write(addr, cmd, 2);
    wait(0.07); // delay 70ms    
}

 
void move_thread(void const *args){

    int angle;
    double length = 40;
    double width = 40;
    int radius = 5;

    Timer t;
    //initally put pen down
    //L.servo(0);
    
    float currentAngle;
    float targetAngle;
    float startAngle;
    float diffAngle;
    
    int steps;
    
    int control;
    int restore = 0;
    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, x_taro_pr, y_taro_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);
        serial.printf("Mode Mover: %i \r\n",mode);

        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_taro_pr = x_taro;
            y_taro_pr = y_taro; 
            x_pen = 0;
            y_pen = 0;
            x_cent = 0;
            y_cent = -1;
            x_taro = 0;
            y_taro = 0;
            
            //set values
            shape_x[4] = 0;
            shape_y[4] = length;
            shape_x[3] = width;
            shape_y[3] = length;
            shape_x[2] = width;
            shape_y[2] = 0;
            shape_x[1] = 0;
            shape_y[1] = 0;
            shape_x[0] = 0;
            shape_y[0] = 0.1;
            shape_count = 5;
            
            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_taro_pr = x_taro;
            y_taro_pr = y_taro; 
            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] = 0;
            shape_x[1] = 0;
            shape_y[1] = 0;
            shape_x[0] = 0;
            shape_y[0] = 0.1;
            
            shape_count = 4;
            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;
            x_taro = 0;
            y_taro = 0;
            
            one_slot.wait();
            counter = 0;
            start = 0;
            one_slot.release();
            
            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 = shape_x[shape_count];
            y_tar = shape_y[shape_count];
            p_mode = 1;
            newval = 1;
            
            //last move -> unblock input
            if(shape_count == 0){
                restore = 1;
                mode = -1;    
            }
        }
        if(newval == 1){
            serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar);
            newval = 0;
            // same position -> do nothing
            if(x_tar == x_pen && y_tar == y_pen){
                
            }
            else{
                //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(65);
                }
                //put pen up
                else if (p_mode == 0){
                    L.servo(55);
                    serial.printf("-- Pen Up\r\n");
                }
                
                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();
                    }
                }
                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();
                    }
                }
                //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
                serial.printf("Distance to be taken: %f\r\n",distance);
                steps = (int) rint(distance); 
                serial.printf("Steps to be taken: %i\r\n",steps);
                for(int i = 0; i< steps; i++){
                    control = step_f();    
                }
                //serial.printf("Reached destination \r\n");
                //update pen and center when at target
                if(restore == 1){
                    serial.printf("Restore previous positions \r\n");
                    x_pen = x_pen_pr;
                    y_pen = y_pen_pr;
                    x_cent = x_cent_pr;
                    y_cent = y_cent_pr;
                    x_taro = x_taro_pr;
                    y_taro = y_taro_pr;
                    restore = 0;    
                }
                else{
                    x_pen = x_tar;
                    y_pen = y_tar;
                    x_cent = x_taro;
                    y_cent = y_taro;
                    x_taro = x_tar;
                    y_taro = y_tar;
                }
            }               
            serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen);
            serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent);    
        }   
    }
}



int main()
{   
    L.reset();
    //L.servo(90);
    wait(1);
    serial.printf("Starting Dead-Reckoning\r\n");
    //ir_localization(); 
    serial.printf("Starting the threads\r\n");
    
    Thread bluetooth(bluetooth_thread);
    Thread move(move_thread);
    mover_id = move.gettid();
    bluetooth_id = bluetooth.gettid();
    
    while(1){
        Thread::wait(3);
        if(which_thread == 0) bluetooth.signal_set(0x1);
        else move.signal_set(0x1);
    }    
}