Diff: robot.h

Revision:

1:8569ac717e68

Parent:

0:15a30802e719

Child:

2:0435d1171673

--- a/robot.h	Thu Feb 02 12:21:11 2017 +0000
+++ b/robot.h	Tue Feb 20 13:37:47 2018 +0000
@@ -1,54 +1,79 @@
 /** @file */
-//AUTOR: Fernando Pais
-//MAIL:  ferpais2508@gmail.com
-//DATA: 6/6/2016
-// VERSÃO 6.4.0
-//
-//Alterações: Problema de compatibilidade entre encoder e infravermelho resolvido
-//            Odometria actualizada automaticamente
-//            Valor da bateria verificado na inicialização
-//            Motores movem-se de 0 a 1000 para melhor difrenciação
-//
 
-//#include "mbed.h"
-//#include "init.h"
-//#define _USE_MATH_DEFINES
-# define M_PI           3.14159265358979323846  /* pi */
 #include <math.h>
 #include <string.h>
 #include "VCNL40x0.h"
-#include "nRF24L01P.h"
+#include <RF24.h>
+#include "Encoder.h"
+
+I2C i2c(PTC9,PTC8);
+I2C i2c1(PTC11,PTC10);
+
+
+    float X=20;
+    float Y=20;
+    
+    Mutex mutex_i2c0, mutex_i2c1;
+    Encoder esquerdo(&i2c, &mutex_i2c0, 0);
+    Encoder direito(&i2c1, &mutex_i2c1, 1);
+    
+    Thread thread;
+    
+
+
+void odometry_thread()
+{
+    float theta=0;
+    long int ticks2d=0;
+    long int ticks2e=0;
+
+
 
-void Odometria();
+    while (true) {
+        esquerdo.incremental();
+        direito.incremental();
+        //-------------------------------------------
+        long int ticks1d=direito.readIncrementalValue();
+        long int ticks1e=esquerdo.readIncrementalValue();
+
+        long int D_ticks=ticks1d - ticks2d;
+        long int E_ticks=ticks1e - ticks2e;
+
+        ticks2d=ticks1d;
+        ticks2e=ticks1e;
+
+        float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
+        float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
+
+        float CM=(D_cm + L_cm)/2;
+
+        theta +=(D_cm - L_cm)/7.18;
+
+        theta = atan2(sin(theta), cos(theta));
+
+        // meter entre 0
+
+        X += CM*cos(theta);
+        Y += CM*sin(theta);
+        //-------------------------------------------
+
+        Thread::wait(10);
+    }
+}
+
 
 // classes adicionais
-nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
+RF24 radio(PTD2, PTD3, PTD1,  PTC12 ,PTC13);
 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
+
+
 Timeout timeout;
 
 Serial pc(PTE0,PTE1);
-I2C i2c(PTC9,PTC8);
-I2C i2c1(PTC11,PTC10);
+
 
 // Variables needed by the lib
 unsigned int  ProxiValue=0;
-short int prev_R=0;
-short int prev_L=0;
-long int total_R=0;
-long int total_L=0;
-long int ticks2d=0;
-long int ticks2e=0;
-float X=20;
-float Y=20;
-float AtractX = 0;
-float AtractY = 0;
-float theta=0;
-int sensor_left=0;
-int sensor_front=0;
-int sensor_right=0;
-short int flag=0;
-int IRobot=0;
-int JRobot=0;
 
 //SAIDAS DIGITAIS (normal)
 DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
@@ -121,11 +146,7 @@
 AnalogOut   dac_comp_mic        (PTE30);        //Dac_Comparator MIC
 
 
-/* Powers up all the VCNL4020. */
-void init_Infrared()
-{
-    VCNL40x0_Device.SetCurrent (20);     // Set current to 200mA
-}
+
 
 /**
  * Selects the wich infrared to comunicate.
@@ -143,25 +164,42 @@
     if(ch>=1)
         ch_f[0]=1<<ch;
 
-    i2c.start();
+mutex_i2c0.lock();
     i2c.write(addr,ch_f,1);
-    i2c.stop();
+    mutex_i2c0.unlock();
+}
+
+
+
+
+
+/* Powers up all the VCNL4020. */
+void init_Infrared()
+{
+    
+    for (int i=0; i<6;i++)
+    {
+            tca9548_select_ch(i);
+            VCNL40x0_Device.SetCurrent (20);     // Set current to 200mA
+    }
+    tca9548_select_ch(0);
 }
 
 
 /**
- * Get ADC value of the chosen infrared.
+ * Get the distance from of the chosen infrared.
  *
- * @param ch - Infrared to read (1..5)
+ * @param ch - Infrared to read (0,1..5)
  *
  * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
  */
-long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
+float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
 {
     tca9548_select_ch(ch);
     VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue);    // read prox value on demand
-
-    return ProxiValue;
+    float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
+    return aux;
+    //return ProxiValue;
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
@@ -256,168 +294,6 @@
 
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////     ENCODER     ///////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-/**
- * Reads Position of left magnetic encoder.
- *
- * @return The absolute position of the left wheel encoder (0..4095)
- */
-long int read_L_encoder()
-{
-
-    char data_r_2[5];
-
-    i2c.start();
-    i2c.write(0x6C);
-    i2c.write(0x0C);
-    i2c.read(0x6D,data_r_2,4,0);
-    i2c.stop();
-
-    short int val1=data_r_2[0];
-    short int val2=data_r_2[1];
-    val1=(val1&0xf)*256;
-    long int final=(val2+val1);
-
-    return  final;
-}
-
-
-/**
- * Reads Position of right magnetic encoder.
- *
- * @return The absolute position of the right wheel encoder (0..4095)
- */
-long int read_R_encoder()
-{
-
-    char data_r_2[5];
-
-    i2c1.start();
-    i2c1.write(0x6C);
-    i2c1.write(0x0C);
-    i2c1.read(0x6D,data_r_2,4,0);
-    i2c1.stop();
-
-    short int val1=data_r_2[0];
-    short int val2=data_r_2[1];
-    val1=(val1&0xf)*256;
-    long int final=(val2+val1);
-
-    return  final;
-}
-
-
-/**
- * Calculates and returns the value of the  right "incremental" encoder.
- *
- * @return The value of "tics" of the right encoder since it was initialized
- */
-long int incremental_R_encoder()
-{
-    short int next_R=read_R_encoder(); // Reads curent value of the encoder
-    short int dif=next_R-prev_R;       // Calculates the diference from last reading
-
-    if(dif>3000) {                     // Going back and pass zero
-        total_R=total_R-4096+dif;
-    }
-    if(dif<3000&&dif>0) {              // Going front
-        total_R=total_R+dif;
-    }
-    if(dif<-3000) {                    // Going front and pass zero
-        total_R=total_R+4096+dif;
-    }
-    if(dif>-3000&&dif<0) {             // going back
-        total_R=total_R+dif;
-    }
-    prev_R=next_R;                     // Sets last reading for next iteration
-
-    return  total_R;
-}
-
-
-/**
- * Calculates and returns the value of the  left "incremental" encoder.
- *
- * @return The value of "tics" of the left encoder since it was initialized
- */
-long int incremental_L_encoder()
-{
-    short int next_L=read_L_encoder(); // Reads curent value of the encoder
-    short int dif=-next_L+prev_L;      // Calculates the diference from last reading
-
-    if(dif>3000) {                     // Going back and pass zero
-        total_L=total_L-4096+dif;
-    }
-    if(dif<3000&&dif>0) {              // Going front
-        total_L=total_L+dif;
-    }
-    if(dif<-3000) {                    // Going front and pass zero
-        total_L=total_L+4096+dif;
-    }
-    if(dif>-3000&&dif<0) {             // going back
-        total_L=total_L+dif;
-    }
-    prev_L=next_L;                     // Sets last reading for next iteration
-
-    return  total_L;
-}
-
-
-/**
- * Calculate the value of both encoder "incremental" every 10 ms.
- */
-void timer_event()  //10ms interrupt
-{
-    timeout.attach(&timer_event,0.01);
-    if(flag==0) {
-        incremental_R_encoder();
-        incremental_L_encoder();
-    }
-    Odometria();
-
-    return;
-}
-
-
-/**
- * Set the initial position for the "incremental" enconder and "starts" them.
- */
-void initEncoders()
-{
-    prev_R=read_R_encoder();
-    prev_L=read_L_encoder();
-    timeout.attach(&timer_event,0.01);
-}
-
-
-/**
- * Returns to the user the value of the right "incremental" encoder.
- *
- * @return The value of "tics" of the right encoder since it was initialized
- */
-long int R_encoder()
-{
-    wait(0.0001);
-
-    return total_R;
-}
-
-/**
- * Returns to the user the value of the right "incremental" encoder.
- *
- * @return The value of "tics" of the right encoder since it was initialized
- */
-long int L_encoder()
-{
-    wait(0.0001);
-
-    return total_L;
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////     BATTERY     ///////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -425,15 +301,14 @@
  * Reads adc of the battery.
  *
  * @param addr - Address to read
- * @return The voltage of the batery
+ * @return The voltage of the baterry
  */
 long int read16_mcp3424(char addr)
 {
     char data[4];
-    i2c1.start();
+    mutex_i2c1.lock();
     i2c1.read(addr,data,3);
-    i2c1.stop();
-
+    mutex_i2c1.unlock();
     return(((data[0]&127)*256)+data[1]);
 }
 
@@ -464,9 +339,9 @@
 
     char data[1];
     data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
-    i2c1.start();
+    mutex_i2c1.lock();
     i2c1.write(addr,data,1);
-    i2c1.stop();
+    mutex_i2c1.unlock();
 }
 
 
@@ -491,179 +366,6 @@
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////     RF COMUNICATION     ///////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-
-/**
- * Initializes the NRF24 module for comunication.
- *
- * Note: if the module is broken or badly connected this init will cause the code to stop,
- *  if all these messages don't appear thats the case
- */
-void config_init_nrf()
-{
-    my_nrf24l01p.powerUp(); // powers module
-    my_nrf24l01p.setRfFrequency (2400); // channel 0 (2400-0 ... 2516-116)
-    my_nrf24l01p.setTransferSize(10);   // number of bytes to be transfer
-    my_nrf24l01p.setCrcWidth(8);
-    my_nrf24l01p.enableAutoAcknowledge(NRF24L01P_PIPE_P0); // pipe where data transfer occurs (0..6)
-    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
-    pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
-    pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
-    pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
-    pc.printf( "nRF24L01+ CrC Width    : %d CrC\r\n", my_nrf24l01p.getCrcWidth() );
-    pc.printf( "nRF24L01+ TransferSize : %d Paket Size\r\n", my_nrf24l01p.getTransferSize () );
-    my_nrf24l01p.setReceiveMode();
-    my_nrf24l01p.enable();
-    pc.printf( "Setup complete, Starting While loop\r\n");
-}
-
-
-/**
- * Receives a number from the Arduino.
- *
- * @return The value send by the arduino
- */
-double receiveValue(void)
-{
-    char temp[4];
-    double Val;
-    bool ok=0;
-    my_nrf24l01p.setTransferSize(4);
-    my_nrf24l01p.setReceiveMode();
-    my_nrf24l01p.enable();
-    do {
-        if(my_nrf24l01p.readable(NRF24L01P_PIPE_P0)) {
-            ok = my_nrf24l01p.read( NRF24L01P_PIPE_P0,temp, 4);
-        }
-    } while(ok==0);
-
-    //transformation of temp to convert to original value
-    if(temp[0]==0) // if first elemente is 0 then its negative
-        Val = double(-(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255));
-    else // else its positive
-        Val = double(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255);
-
-    return Val;
-}
-
-
-/**
- * Sends a number to the Arduino
- *
- * @param Value - number to be sent to the Arduino
- */
-void sendValue(long int Value)
-{
-    bool ok=0;  // comunication sucess, o if failed 1 if sucessfull
-    // double math=Value/65025; // temporary variable save results
-    int zero=1;  // 1 byte, ( - ) if 0 ( + ) if 1
-    int one=0;   // 2 byte (0..255), multiplied by 1
-    int two=0;   // 3 byte (0..255), multiplied by 255
-    int three=0; // 4 byte (0..255), multiplied by 255*255
-
-//transformation of Value to send correctly through pipe
-    if (Value<0) {
-        zero=0;
-        Value=abs(Value);
-    }
-    //  Value=abs(Value);
-
-    double math=Value/65025; // temporary variable save results
-
-    if(math<1) {
-        math=Value/255;
-        if(math<1) {
-            two=0;
-            one=Value;
-        } else {
-            two=(int)math;
-            one=Value % 255;
-        }
-    } else {
-        three=(int)math;
-        math=Value/255;
-        if(math<1) {
-            two=0;
-            one=Value;
-        } else {
-            two=(int)math;
-            one=Value % 255;
-        }
-    }
-    char temp[4]= {(int)zero,(int)one,(int)two,(int)three};
-
-    // Apagar depois de testar mais vezes
-    // pc.printf("1 inidice...%i...\r", temp[0]);
-    // pc.printf("2 inidice...%i...\r", temp[1]);
-    // pc.printf("3 inidice...%i...\r", temp[2]);
-    // pc.printf("4 inidice...%i...\r", temp[3]);
-
-    my_nrf24l01p.setTransferSize(4);
-    my_nrf24l01p.setTransmitMode();
-    my_nrf24l01p.enable();
-    do {
-        ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,temp, 4);
-        if(ok==1)
-            pc.printf("Done.....%i...\r", Value);
-        else {
-            pc.printf("Failed\r");
-            wait(1);
-        }
-    } while(ok==0);
-}
-
-/**
- *  Sends matrix to arduino.
- *
- * @param matrix - Matrix of numbers to send [0..255]
- * @param row - Number of rows
- * @param column - Number of columns
- */
-void sendMatrix(int (*matrix)[18],int row , int column)
-{
-    short ok=0;
-    short int i =0;
-    short int j=0;
-    short int byte=0;
-    int members=column*row;
-    char message[32]= {0};
-    pc.printf("J ...%d... \r",members);
-
-    my_nrf24l01p.setTransferSize(32);
-    my_nrf24l01p.setTransmitMode();
-    my_nrf24l01p.enable();
-
-    do {
-        int* point = matrix[j];
-
-        do {
-            message[byte]= point[i];
-            if (byte==31 || (i+1)*(j+1)==members) {
-
-                do {
-                    ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,message, 32);
-                    if(ok==0)
-                        wait(1);
-
-                } while(ok==0);
-
-                byte=-1;
-            }
-
-            byte++;
-            i++;
-
-        } while(i<column);
-
-        i=0;
-        j++;
-    } while(j<row);
-
-}
-
-///////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////      Sonar     ////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////
 //      Commands of operation with ultrasonic module
@@ -688,57 +390,9 @@
 {
     char data[1];
     data[0]= 0x0C;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data,1);
-    i2c1.stop();
-    i2c1.start();
-    i2c1.write(0x30,data,1);
-    i2c1.stop();
-}
-
-
-void disable_dc_dc_boost()
-{
-    char data[1];
-    data[0]= 0x0B;
-    wait_ms(1);
-    i2c1.start();
+    mutex_i2c1.lock();
     i2c1.write(0x30,data,1);
-    i2c1.stop();
-}
-
-
-void start_read_left_sensor()
-{
-    char data[1];
-    data[0]= 0x0A;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data,1);
-    i2c1.stop();
-}
-
-
-void start_read_front_sensor()
-{
-    char data[1];
-    data[0]= 0x09;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data,1);
-    i2c1.stop();
-}
-
-
-void start_read_right_sensor()
-{
-    char data[1];
-    data[0]= 0x08;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data,1);
-    i2c1.stop();
+    mutex_i2c1.unlock();
 }
 
 
@@ -746,49 +400,30 @@
 {
     char data[1];
     data[0]= 0x07;
-    wait_ms(1);
-    i2c1.start();
+    mutex_i2c1.lock();
     i2c1.write(0x30,data,1);
-    i2c1.stop();
+    mutex_i2c1.unlock();
 }
 
 
-void measure_always_off()
-{
-    char data[1];
-    data[0]= 0x06;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data,1);
-    i2c1.stop();
-}
-
 /**
  * Returns left sensor value
  */
 static unsigned int get_distance_left_sensor()
 {
-
+      
     static char data_r[3];
     static unsigned int aux;
-    flag=1;
-
+    
     data_r[0]= 0x05;
-    wait_ms(1);
-    i2c1.start();
+    mutex_i2c1.lock();
     i2c1.write(0x30,data_r,1);
-    i2c1.stop();
-    wait_ms(10);
-    i2c1.start();
-    i2c1.read(0x31,data_r,2,0);
-    i2c1.stop();
+    i2c1.read(0x31,data_r,2);
+    mutex_i2c1.unlock();
 
     aux=(data_r[0]*256)+data_r[1];
-    flag=0;
+
     return aux;
-    // sensor_left=aux;
-    // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
-
 }
 
 
@@ -800,22 +435,17 @@
 
     static char data_r[3];
     static unsigned int aux;
-    flag=1;
+
     data_r[0]= 0x04;
-    wait_ms(1);
-    i2c1.start();
+    
+    mutex_i2c1.lock();
     i2c1.write(0x30,data_r,1);
-    i2c1.stop();
-    wait_ms(10);
-    i2c1.start();
-    i2c1.read(0x31,data_r,2,0);
-    i2c1.stop();
+    i2c1.read(0x31,data_r,2);
+    mutex_i2c1.unlock();
 
     aux=(data_r[0]*256)+data_r[1];
-    flag=0;
+
     return aux;
-    // sensor_front=aux;
-    // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
 
 }
 
@@ -828,69 +458,359 @@
 
     static char data_r[3];
     static unsigned int aux;
-    flag=1;
 
     data_r[0]= 0x03;
-    wait_ms(1);
-    i2c1.start();
+
+    mutex_i2c1.lock();
     i2c1.write(0x30,data_r,1);
-    i2c1.stop();
-    wait_ms(10);
-    i2c1.start();
     i2c1.read(0x31,data_r,2,0);
-    i2c1.stop();
+    mutex_i2c1.unlock();
 
     aux=(data_r[0]*256)+data_r[1];
-    flag=0;
+
     return aux;
-    // sensor_right=aux;
-    // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
 
 }
 
 
-void get_status_always_measure()
-{
+///////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////     RF COMMUNICATION      ////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
 
-    static char data_r[3];
-    static unsigned int aux;
+/**
+ * Initializes nrf24l01 module, return value 1 if module is working correcty and 0 if it's not.
+ * 
+ ** @param channel - Which RF channel to comunicate on, 0-125
+ *
+ * \warning Channel on Robot and Board has to be the same.
+ */
+ void init_nRF(int channel){
+        int result;
+      result = radio.begin();
+      
+      pc.printf( "Initialation nrf24l01=%d\r\n",  result ); // 1-working,0-not working
+      radio.setDataRate(RF24_1MBPS);
+      radio.setCRCLength(RF24_CRC_16);
+      radio.setPayloadSize(32);
+      radio.setChannel(channel);
+      radio.setAutoAck(true);
+    
+      radio.openWritingPipe(0x314e6f6465);
+      radio.openReadingPipe(1,0x324e6f6465);
+    
+      radio.startListening();
+}
+
+char txData, rxData;
+char q[1];
+
 
-    data_r[0]= 0x02;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data_r,1);
-    i2c1.stop();
-    wait_ms(10);
-    i2c1.start();
-    i2c1.read(0x31,data_r,2,0);
-    i2c1.stop();
+/**
+ * Wireless control of motors using nrf24l01 module and FRDM-KL25Z board(robot is working as receiver).
+ */
+void robot_RC()
+{
+   if (radio.available() ) {
 
-    aux=data_r[0];
-    pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
-
+            radio.read( &rxData, sizeof(rxData) );
+            pc.putc( rxData );
+            
+            q[0]=pc.putc( rxData );
+            if (q[0]=='w'){
+                leftMotor(1,500);
+                rightMotor(1,500);
+                }
+            else if( q[0]=='s'){
+                leftMotor(0,500);
+                rightMotor(0,500);
+                } 
+            else if( q[0]=='a'){
+                leftMotor(0,500);
+                rightMotor(1,500);
+                } 
+            else if( q[0]=='d'){
+                leftMotor(1,500);
+                rightMotor(0,1000);
+                } 
+            else if( q[0]==' '){
+                leftMotor(1,0);
+                rightMotor(1,0);
+                }                          
+        }
 }
 
 
-void get_status_dcdc_converter()
-{
+
+/**
+ * Robot is sending values of all sensors (right/left incremental encoder, ultrasonic sensors, infarated sensors and Odometria) to FRDM-KL25Z board using nrf24l01 module.
+ *
+ *Note: Check if module is initilized correctly.
+ */
+void send_Robot_values(){
+                
+//                      WRITE ID:
+//          INCREMENTAL LEFT ENCODER        - a
+//          INCREMENTAL RIGHT ENCODER       - b
+//          LEFT ULTRASENSOR                - c
+//          FRONT ULTRASENSOR               - d  
+//          RIGHT ULTRASENSOR               - e     
+//          INFRARED SENSOR 0               - f  
+//          INFRARED SENSOR 1               - g  
+//          INFRARED SENSOR 2               - h  
+//          INFRARED SENSOR 3               - i  
+//          INFRARED SENSOR 4               - j  
+//          INFRARED SENSOR 5               - k  
+//          ODOMETRIA "X"                   - l               
+//          ODOMETRIA "Y"                   - m                
+                
+//      LEFT ENCODER        addr: a              
+                                
+    int left_encoder = 0;              //read value from device 
+    char left_encoder_R[10];            //char array ..._R to read value
+    char left_encoder_W[12];            //char array ..._W to write value          
+                              
+    snprintf(left_encoder_R, 10, "%d", left_encoder);              //int value to char array
+
+    for (int i=0; i < 10;i++)         //changing posiision of chars in array
+        {
+            left_encoder_W[i+2]=left_encoder_R[i];
+        }
+    
+    left_encoder_W[0]='Q';               //adding id to message        
+    left_encoder_W[1]='a';               //adding id to parameter 
+
+//      RIGHT ENCODER       addr: b                  
+
+    int right_encoder = 0;            //read value from device  
+    char right_encoder_R[10];            //char array ..._R to read value
+    char right_encoder_W[11];            //char array ..._W to write value          
+                             
+    snprintf(right_encoder_R, 10, "%d", right_encoder);              //int value to char array
+
+    for (int i=0; i <10;i++)         //changing positision of chars in array
+        {
+            right_encoder_W[i+1]=right_encoder_R[i];
+        }
+                 
+    right_encoder_W[0]='b';               //adding id to parameter
+
+
+//      LEFT ULTRASENSOR       addr: c                  
+
+    int left_sensor = get_distance_left_sensor();            //read value from device 
+    char left_sensor_R[5];            //char array ..._R to read value
+    char left_sensor_W[6];            //char array ..._W to write value          
+                             
+    snprintf(left_sensor_R, 5, "%d", left_sensor);              //int value to char array
 
-    static char data_r[3];
-    static unsigned int aux;
+    for (int i=0; i < 5;i++)         //changing positision of chars in array
+        {
+            left_sensor_W[i+1]=left_sensor_R[i];
+        }
+                 
+    left_sensor_W[0]='c';               //adding id to parameter
+
+
+//      FRONT ULTRASENSOR       addr: d                  
+
+    int front_sensor = get_distance_front_sensor();            //read value from device 
+    char front_sensor_R[5];            //char array ..._R to read value
+    char front_sensor_W[6];            //char array ..._W to write value          
+                             
+    snprintf(front_sensor_R, 5, "%d", front_sensor);              //int value to char array
+
+    for (int i=0; i < 5;i++)         //changing positision of chars in array
+        {
+            front_sensor_W[i+1]=front_sensor_R[i];
+        }
+                 
+    front_sensor_W[0]='d';               //adding id to parameter
+
+
+//      RIGHT ULTRASENSOR       addr: e                  
+
+    int right_sensor = get_distance_right_sensor();            //read value from device
+    char right_sensor_R[5];            //char array ..._R to read value
+    char right_sensor_W[6];            //char array ..._W to write value          
+                             
+    snprintf(right_sensor_R, 5, "%d", right_sensor);              //int value to char array
+
+    for (int i=0; i < 5;i++)         //changing positision of chars in array
+        {
+            right_sensor_W[i+1]=right_sensor_R[i];
+        }
+                 
+    right_sensor_W[0]='e';               //adding id to parameter
+    
+
+//     INFRARED SENSOR 0       addr: f                  
+
+    float infrared_0 = read_Infrared(0);            //read value from device
+     
+    char infrared_0_R[5];            //char array ..._R to read value
+    char infrared_0_W[7];            //char array ..._W to write value          
+                             
+    snprintf(infrared_0_R, 5, "%.1f", infrared_0);              //int value to char array
+
+    for (int i=0; i < 6;i++)         //changing positision of chars in array
+        {
+            infrared_0_W[i+2]=infrared_0_R[i];
+        }
+                 
+    infrared_0_W[0]='Z';               //adding id to message
+    infrared_0_W[1]='f';               //adding id to parameter
+
+//     INFRARED SENSOR 1       addr: g                  
+
+    float infrared_1 = read_Infrared(1);            //read value from device
+     
+    char infrared_1_R[sizeof(infrared_1)+1];            //char array ..._R to read value
+    char infrared_1_W[sizeof(infrared_1)+2];            //char array ..._W to write value          
+                             
+    snprintf(infrared_1_R, sizeof(infrared_1_R), "%.1f", infrared_1);              //int value to char array
+
+    for (int i=0; i < sizeof(infrared_1)+2;i++)         //changing positision of chars in array
+        {
+            infrared_1_W[i+1]=infrared_1_R[i];
+        }
+                 
+    infrared_1_W[0]='g';               //adding id to parameter
 
-    data_r[0]= 0x01;
-    wait_ms(1);
-    i2c1.start();
-    i2c1.write(0x30,data_r,1);
-    i2c1.stop();
-    wait_ms(10);
-    i2c1.start();
-    i2c1.read(0x31,data_r,2,0);
-    i2c1.stop();
+
+//     INFRARED SENSOR 2       addr: h                  
+
+    float infrared_2 = read_Infrared(2);            //read value from device
+     
+    char infrared_2_R[sizeof(infrared_2)+1];            //char array ..._R to read value
+    char infrared_2_W[sizeof(infrared_2)+2];            //char array ..._W to write value          
+                             
+    snprintf(infrared_2_R, sizeof(infrared_2_R), "%.1f", infrared_2);              //int value to char array
+
+    for (int i=0; i < sizeof(infrared_2)+2;i++)         //changing positision of chars in array
+        {
+            infrared_2_W[i+1]=infrared_2_R[i];
+        }
+                 
+    infrared_2_W[0]='h';               //adding id to parameter
+    
+    
+//     INFRARED SENSOR 3       addr: i                  
+
+    float infrared_3 = read_Infrared(3);            //read value from device
+     
+    char infrared_3_R[sizeof(infrared_3)+1];            //char array ..._R to read value
+    char infrared_3_W[sizeof(infrared_3)+2];            //char array ..._W to write value          
+                             
+    snprintf(infrared_3_R, sizeof(infrared_3_R), "%.1f", infrared_3);              //int value to char array
+
+    for (int i=0; i < sizeof(infrared_3)+2;i++)         //changing positision of chars in array
+        {
+            infrared_3_W[i+1]=infrared_3_R[i];
+        }
+                 
+    infrared_3_W[0]='i';               //adding id to parameter
+    
+    
+//     INFRARED SENSOR 4       addr: j                  
+
+    float infrared_4 = read_Infrared(4);            //read value from device
+     
+    char infrared_4_R[sizeof(infrared_4)+1];            //char array ..._R to read value
+    char infrared_4_W[sizeof(infrared_4)+2];            //char array ..._W to write value          
+                             
+    snprintf(infrared_4_R, sizeof(infrared_4_R), "%.1f", infrared_4);              //int value to char array
+
+    for (int i=0; i < sizeof(infrared_4)+2;i++)         //changing positision of chars in array
+        {
+            infrared_4_W[i+1]=infrared_4_R[i];
+        }
+                 
+    infrared_4_W[0]='j';               //adding id to parameter 
+
+
+//     INFRARED SENSOR 5       addr: k                  
+
+    float infrared_5 = read_Infrared(5);            //read value from device
+     
+    char infrared_5_R[sizeof(infrared_5)+1];            //char array ..._R to read value
+    char infrared_5_W[sizeof(infrared_5)+2];            //char array ..._W to write value          
+                             
+    snprintf(infrared_5_R, sizeof(infrared_5_R), "%.1f", infrared_5);              //int value to char array
 
-    aux=data_r[0];
-    pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
+    for (int i=0; i < sizeof(infrared_5)+2;i++)         //changing positision of chars in array
+        {
+            infrared_5_W[i+1]=infrared_5_R[i];
+        }
+                 
+    infrared_5_W[0]='k';               //adding id to parameter 
+   
+  
+//     ODEMETRIA X             addr: l                  
+
+    float X_od = X;            //read value from device
+     
+    char X_od_R[sizeof(X_od)+1];            //char array ..._R to read value
+    char X_od_W[sizeof(X_od)+2];            //char array ..._W to write value          
+                             
+    snprintf(X_od_R, sizeof(X_od_R), "%.1f", X_od);              //int value to char array
+
+    for (int i=0; i < sizeof(X_od)+2;i++)         //changing positision of chars in array
+        {
+            X_od_W[i+1]=X_od_R[i];
+        }
+                 
+    X_od_W[0]='l';               //adding id to parameter 
+ 
+
+//     ODEMETRIA X              addr: m                  
+
+    float Y_od = Y;            //read value from device
+     
+    char Y_od_R[sizeof(Y_od)+1];            //char array ..._R to read value
+    char Y_od_W[sizeof(Y_od)+2];            //char array ..._W to write value          
+                             
+    snprintf(Y_od_R, sizeof(Y_od_R), "%.1f", Y_od);              //int value to char array
 
-}
+    for (int i=0; i < sizeof(Y_od)+2;i++)         //changing positision of chars in array
+        {
+            Y_od_W[i+1]=Y_od_R[i];
+        }
+                 
+    Y_od_W[0]='m';               //adding id to parameter   
+   
+    
+    
+/////Preparing messages to send//////
+
+char send_Z[30];
+for (int i =0; i<sizeof(left_encoder_W);i++){
+    send_Z[i]=left_encoder_W[i];
+    }
+strcat(send_Z,right_encoder_W);
+strcat(send_Z,left_sensor_W);
+strcat(send_Z,front_sensor_W);
+strcat(send_Z,right_sensor_W);
+strcat(send_Z,X_od_W);
+
+char send_Q[40];
+for (int i =0; i<sizeof(infrared_0_W);i++){
+    send_Q[i]=infrared_0_W[i];
+    }
+strcat(send_Q,infrared_1_W);
+strcat(send_Q,infrared_2_W);
+strcat(send_Q,infrared_3_W);
+strcat(send_Q,infrared_4_W);
+strcat(send_Q,infrared_5_W);
+strcat(send_Q,Y_od_W);
+
+
+
+/////Sending messages//////
+        radio.stopListening();
+        radio.write( &send_Z, sizeof(send_Z));
+        radio.write( &send_Q, sizeof(send_Q) );
+        radio.startListening();
+    }
 
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
@@ -958,70 +878,42 @@
     pwm_buzzer.period_us(500);
     pwm_buzzer.pulsewidth_us(0);
 
+
     //LED white
     pwm_led_whi.period_us(500);
     pwm_led_whi.pulsewidth_us(0);
 
 }
 
-
 /**
  * Initializes all the pins and all the modules necessary
+ *
+ * @param channel - Which RF channel to comunicate on, 0-125.
+ *\note If you are not using RF module put random number.
+ * \warning Channel on Robot and Board has to be the same.
  */
-void initRobot(void)
-{
+void initRobot(int channel)
+{   
+    pc.printf("Battery level: \n\r");
+    init_nRF(channel);
     init_robot_pins();
     enable_dc_dc_boost();
-    init_Infrared();
-    initEncoders();
-    config_init_nrf();
+    wait_ms(100); //wait for read wait(>=150ms);
     enable_dc_dc_boost();
+    measure_always_on();
     wait_ms(100); //wait for read wait(>=150ms);
-    measure_always_on();
+   
+    init_Infrared();
     float value = value_of_Batery();
+    
+    thread.start(odometry_thread);
+    
     pc.printf("Initialization Successful \n\r");
     pc.printf("Battery level: %f \n\r",value);
     if(value < 3.0) {
         pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
     }
-
     // float level = value_of_Batery();
     // sendValue(int(level*100));
 
-}
-
-
-////////////////////////////////////////////////////
-
-/**
- * Updates the position and orientation of the robot based on the data from the encoders
- *
- * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
- * and the distance between them is 7.4
- */
-void Odometria()
-{
-    long int ticks1d=R_encoder();
-    long int ticks1e=L_encoder();
-
-    long int D_ticks=ticks1d - ticks2d;
-    long int E_ticks=ticks1e - ticks2e;
-
-    ticks2d=ticks1d;
-    ticks2e=ticks1e;
-
-    float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
-    float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
-
-    float CM=(D_cm + L_cm)/2;
-
-    theta +=(D_cm - L_cm)/7.18;
-
-    theta = atan2(sin(theta), cos(theta));
-
-    // meter entre 0
-
-    X += CM*cos(theta);
-    Y += CM*sin(theta);
-
 }
\ No newline at end of file