File content as of revision 14:82248fb06e53:

// main of SCRIBE servo 

// Import libraries 
#include "Arduino.h"
#include "BLEPeripheral.h"
#include "mbed.h"
#include "rtos.h"
#include "localization.h"
#include "servo.h"
 
Serial serial(USBTX, USBRX);

//Analog In for ADC A0 -> p15
AnalogIn  ain1(A0);
AnalogIn  ain2(A1);

//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
I2C i2c(p9, p10);        // sda, scl
const int addr = 0xB0;   // define the I2C Address of camera
int ir_finish = 0;

void back_to_zero();
 
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;
double x_taro = 0;
double y_taro = 0;

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


Semaphore one_slot(1);

localization L;

osThreadId bluetooth_id, mover_id,ir_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){
                            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");
        }
    }
}

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 clock_init()
{
    // set up ~20-25MHz clock on p21
    LPC_PWM1->TCR = (1 << 1);               // Reset counter, disable PWM
    LPC_SC->PCLKSEL0 &= ~(0x3 << 12);  
    LPC_SC->PCLKSEL0 |= (1 << 12);          // Set peripheral clock divider to /1, i.e. system clock
    LPC_PWM1->MR0 = 4;                     // Match Register 0 is shared period counter for all PWM1
    LPC_PWM1->MR6 = 2;                      // Pin 21 is PWM output 6, so Match Register 6
    LPC_PWM1->LER |= 1;                     // Start updating at next period start
    LPC_PWM1->TCR = (1 << 0) || (1 << 3);   // Enable counter and PWM
}
 
void cam_init()
{
    // Init IR Camera sensor
    i2c_write2(addr, 0x30, 0x01);
    i2c_write2(addr, 0x30, 0x08);    
    i2c_write2(addr, 0x06, 0x90);
    i2c_write2(addr, 0x08, 0xC0);
    i2c_write2(addr, 0x1A, 0x40);
    i2c_write2(addr, 0x33, 0x33);
    wait(0.1);
}
 
void ir_localization(void const *args) {
    ir_id = Thread::gettid();
    char cmd[8];
    char buf[16];
    int Ix1,Iy1,Ix2,Iy2;
    int Ix3,Iy3,Ix4,Iy4;
    double angle1,angle2,angle3,angle4;
    double angle_init, sum_x;
    int s;
    
    // variables for localization
    int m_count = 0;
    double alpha1,alpha2,phi1,phi2,phiIMU1,phiIMU2,beta;
    double x,y,z;
    
    // height is given
    double h = 97;
    
    //clock_init();
    
    // PC serial output    
    serial.printf("Initializing camera...");
 
    cam_init();
  
    serial.printf("complete\n");
    
    float testAngle = L.getAngle();
    angle_init = L.getAngle();
    
    serial.printf("Initial IMU-Angle is: %f\r\n",angle_init);
    servo_slowleft();
    // read I2C stuff
    while(m_count !=2){
        // IR Sensor read
        cmd[0] = 0x36;
        i2c.write(addr, cmd, 1);
        i2c.read(addr, buf, 16); // read the 16-byte result
        
        Ix1 = buf[1];
        Iy1 = buf[2];
        s   = buf[3];
        Ix1 += (s & 0x30) <<4;
        Iy1 += (s & 0xC0) <<2;
        
        Ix2 = buf[4];
        Iy2 = buf[5];
        s   = buf[6];
        Ix2 += (s & 0x30) <<4;
        Iy2 += (s & 0xC0) <<2;
        
        Ix3 = buf[7];
        Iy3 = buf[8];
        s   = buf[9];
        Ix3 += (s & 0x30) <<4;
        Iy3 += (s & 0xC0) <<2;
        
        Ix4 = buf[10];
        Iy4 = buf[11];
        s   = buf[12];
        Ix4 += (s & 0x30) <<4;
        Iy4 += (s & 0xC0) <<2;
        
        angle1 = 45*((double)Ix1 / 1023 - 0.5);
        angle2 = 45*((double)Ix2 / 1023 - 0.5);
        angle3 = 45*(double)Ix3 / 1023;
        angle4 = 45*(double)Ix4 / 1023;
        
        // print the coordinate data
        serial.printf("Ix1: %4d, Iy1: %4d\n", Ix1, Iy1 );
        //serial.printf("Ix2: %4d, Iy2: %4d\n", Ix2, Iy2 );
        //serial.printf("Current IMU-Angle is: %f\r\n",L.getAngle());
        //print when more than one LED present
        
        if(Ix1 < 1023){
            //serial.printf("%d, %d: %f; %d, %d : %f\r\n", Ix1, Iy1, angle1, Ix2, Iy2, angle2);
/*            if(angle1 <= -2){
                //serial.printf("Turn left! \r\n");
            }
            else if (angle1 >= 2 && (abs(360 - L.getAngle()) > 90)){
                serial.printf("Turn right! \r\n");
            }*/
            if(abs(angle1) < 2) {
                if(m_count == 0 && abs(360 - L.getAngle()) < 90){
                    sum_x = 0;
                    // get ten pictures
                    for(int i = 0; i < 10; i++){
                        cmd[0] = 0x36;
                        i2c.write(addr, cmd, 1);
                        i2c.read(addr, buf, 16);
                        
                        Ix1 = buf[1];
                        Iy1 = buf[2];
                        s   = buf[3];
                        Ix1 += (s & 0x30) <<4;
                        Iy1 += (s & 0xC0) <<2;
                        angle1 = 45*((double)Ix1 / 1023 - 0.5);  
                        sum_x = sum_x + angle1;  
                    }
                    serial.printf("*** Got average angle1: %f\r\n",sum_x/10);
                    phiIMU1 = L.getAngle();
                    phi1 = 360 - (phiIMU1 + sum_x/10);
                    serial.printf("*** Got phi1: %f, got phi_IMU1: %f\r\n", phi1,phiIMU1);
                    m_count = 1;
                }
                else if (m_count == 1 && abs(360 - L.getAngle()) > 90){
                    servo_stop();
                    phiIMU2 = L.getAngle();
                    
                    sum_x = 0;
                    // get ten pictures
                    for(int i = 0; i < 10; i++){
                        cmd[0] = 0x36;
                        i2c.write(addr, cmd, 1);
                        i2c.read(addr, buf, 16);
                        
                        Ix1 = buf[1];
                        Iy1 = buf[2];
                        s   = buf[3];
                        Ix1 += (s & 0x30) <<4;
                        Iy1 += (s & 0xC0) <<2;
                        angle2 = 45*((double)Ix1 / 1023 - 0.5);
                        
                        sum_x = sum_x + angle2;  
                    }
                    serial.printf("*** Got average angle2: %f\r\n",sum_x/10);
                    
                    phi2 = 360 - (phiIMU2 + sum_x/10);
                    serial.printf("*** Got phi2: %f, got phi_IMU2: %f\r\n", phi2,phiIMU2);
                    alpha1 = 90 - phi1;
                    alpha2 = 90 - alpha1;
                    beta = 180 - (phi2 - phi1) - alpha2;
                    z = h*sin(beta/180*3.1415)/sin((phi2 - phi1)/180*3.1415);
                    x = z*sin(phi1/180*3.1415);
                    y = z*sin(alpha1/180*3.1415);
                    
                    serial.printf("Angles are: phi1 = %f, phi2 = %f:\r\n",phi1, phi2);
                    serial.printf("Angles are: alpha1 = %f, alpha2 = %f:\r\n",alpha1, alpha2);
                    
                    //reset after print out and set coordinates
                    m_count = 2;
                    x_pen = rint(x/4.5);
                    y_pen = rint((h-y)/4.5);
                    
                    //correction for translation
                    x_pen = x_pen + 2;
                    y_pen = y_pen - 2;
                    x_cent = x_pen;
                    y_cent = y_pen - 1;
                    x_taro = x_pen;
                    y_taro = y_pen;  
                    serial.printf("Current location: x = %f, y = %f \r\n",x,h-y);
                    serial.printf("Current coordinates Pen: x = %f, y = %f \r\n",x_pen,y_pen);
                    serial.printf("Current coordinates Center: x = %f, y = %f \r\n",x_cent,y_cent);
                }
            }
            
        }
        wait(0.2);
    }
    
    //turn upside again
    back_to_zero();
    ir_finish = 1;
}

void back_to_zero(){
    float testAngle = L.getAngle();
    serial.printf("Current Angle BTZ: %f",L.getAngle());
    while(L.getAngle()> 1){
        if(L.getAngle() > 180){
            servo_slowright();        
        }
        else{
            servo_slowleft();
        }
    }
    servo_stop();
}
 
void move_thread(void const *args){

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

    Timer t;
    //initally put pen down
    
    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 = 2;
    double time_factor = 0.05;
    
    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;
    serial.printf("Started move thread\r\n");
    int newval = 0;
    
    //values for encoding
    int counter_l = 0;
    int on_l;
    int counter_r = 0;
    int on_r;
    double sum_r = 0;
    double sum_l = 0;
    double value_r;
    double value_l;
    float angle_init;
    
    float testAngle = L.getAngle();

    while(1){
        // check what mode is present
        serial.printf("Cylce \r\n");
        serial.printf("mode: %i \r\n",mode);
        
        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_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 = 0.08;
            }
            first = 1;
        }
        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 = (double) path_x[start];
                y_tar = (double) path_y[start];
                serial.printf("Got data: x = %f, y = %f \r\n",x_tar,y_tar);
                x_tar = x_tar*draw_corr;
                y_tar = y_tar*draw_corr;
                serial.printf("Scaled data: x = %f, y = %f \r\n",x_tar,y_tar);
                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(30);
                }
                //put pen up
                else if (p_mode == 0){
                    L.servo(45);
                    serial.printf("-- Pen Up\r\n");
                }
                
                currentAngle = L.getAngle();
                if(dot > 0){
                    //serial.printf("Turn right \r\n");
                    targetAngle = fmod(currentAngle+angle,360);
                    
                    servo_right();
                    while(fmod(abs(L.getAngle() - targetAngle),360)> 5);
                    servo_stop();
                }
                else if(dot < 0){
                    //serial.printf("Turn left \r\n");
                    targetAngle = fmod(currentAngle-angle,360);
                    servo_left();
                    while(fmod(abs(L.getAngle() - targetAngle),360)> 5);
                    servo_stop();
                }
                //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                
                int counter_m = (int) rint(distance);
                
                // do adjustment when going down
                if (L.getAngle() > 330 || L.getAngle() < 30){
                    counter_m = counter_m + 2;    
                }
                if (L.getAngle() > 130 || L.getAngle() < 230){
                    counter_m = counter_m - 2;    
                }
                serial.printf("*** Distance to be taken: %d\r\n",counter_m);

                // find position of wheels
                for (int i = 0; i < 100; i++){
                        sum_r += ain1.read();
                        sum_l += ain2.read();  
                }
                value_r = sum_r/100;
                value_l = sum_l/100;
                     
                if(value_r < 0.93){
                    on_r = 0;
                }
                else{
                    on_r = 1;
                }
                if(value_l < 0.925){
                    on_l = 0;
                }
                else{
                    on_l = 1;
                }
                serial.printf("Initially: left: %d, right: %d\r\n",on_l,on_r);
                angle_init = L.getAngle();
                servo_f();
                serial.printf("Initial Angle: %f\r\n",angle_init);
                //start decrement counter
                counter_r = counter_m;
                counter_l = counter_m;
                while (counter_m > 0){
                    sum_r = 0;
                    sum_l = 0;
                    for (int i = 0; i < 5; i++){
                        sum_r += ain1.read();  
                        sum_l += ain2.read(); 
                    }
                    value_r = sum_r/5;
                    value_l = sum_l/5;
                    printf("Left Value: %f\r\n",value_l);
                    printf("Right Value: %f\r\n",value_r);
                    if(value_r < 0.84 && on_r == 1){
                        //printf("Value right: %f\r\n",value_r);
                        on_r = 0;
                    }
                    else if (value_r > 0.88 && on_r == 0){
                        //printf("Value right: %f\r\n",value_r);
                        on_r = 1;
                        counter_r--;
                        printf("*** Right Counter is: %d\r\n",counter_r);
                    }
                    if(value_l < 0.9 && on_l == 1){
                        //printf("Value left: %f\r\n",value_l);
                        on_l = 0;
                    }
                    else if (value_l > 0.92 && on_l == 0){
                        //printf("Value left: %f\r\n",value_l);
                        on_l = 1;
                        counter_l--;
                        printf("*** Left Counter is: %d\r\n",counter_l);
                    }
                    
                    if(counter_l >= counter_r){
                        counter_m = counter_r;
                    }
                    else{
                        counter_m = counter_l;    
                    }
                    //deviation left from path
                    if(angle_init > 270 || angle_init < 90){
                        if((L.getAngle() - angle_init > 2 && L.getAngle() - angle_init < 10) || (angle_init > 358 &&  L.getAngle() < 5 && (L.getAngle() + 360 - angle_init) > 2)){
                            servo_slowleft();
                            serial.printf("Left: Angle Difference: %f\r\n",L.getAngle() - angle_init);
                            serial.printf("Left: Init: %f, Current: %f\r\n",angle_init,L.getAngle());
                            while(fmod(abs(L.getAngle() - angle_init),360)> 1);
                            servo_f();                        
                        }
                        //deviation right from path
                        if(angle_init - L.getAngle() > 2 || (L.getAngle() > 358 &&  angle_init < 5 && (angle_init + 360 - L.getAngle()) > 2)){
                            servo_slowright();
                            while(fmod(abs(L.getAngle() - angle_init),360)> 1);
                            servo_f();                        
                        }
                    }
                    //printf("*** Distance to take is: %d\r\n",counter_m);
                }
                //servo_f();
                //wait(distance*time_factor);
                servo_stop();
                //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();
    servo_reset();
    servo_stop();
    serial.printf("Starting Calibration\r\n");
    back_to_zero();

    serial.printf("Starting IR-Localization\r\n");
    //ir_localization(); 
    Thread ir(ir_localization);
    ir_id = ir.gettid();
    
    while(ir_finish == 0){
        Thread::wait(10);
    }
    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);
    }    
}