File content as of revision 1:8569ac717e68:

/** @file */

#include <math.h>
#include <string.h>
#include "VCNL40x0.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;



    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
RF24 radio(PTD2, PTD3, PTD1,  PTC12 ,PTC13);
VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);


Timeout timeout;

Serial pc(PTE0,PTE1);


// Variables needed by the lib
unsigned int  ProxiValue=0;

//SAIDAS DIGITAIS (normal)
DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
DigitalOut  q_sleep_mot_rig     (PTE3,0);     //Nano Sleep Motor Right
DigitalOut  q_pha_mot_lef       (PTA17,0);    //Phase Motor Left
DigitalOut  q_sleep_mot_lef     (PTB11,0);    //Nano Sleep Motor Left
DigitalOut  q_pow_ena_i2c_p     (PTE2,0);     //Power Enable i2c FET P (0- enable 1-disable)
DigitalOut  q_pow_ena_mic_p     (PTA14,0);    //Power enable Micro FET P (0- enable 1-disable)
DigitalOut  q_pow_as5600_n      (PTC6,1);     //AS5600 Power MOSFET N (1- enable 0-disable)
DigitalOut  q_pow_as5600_p      (PTC5,0);     //AS5600 Power MOSFET P (0- enable 1-disable)
DigitalOut  q_pow_spi           (PTC4,0);     //SPI Power MOSFET P (0- enable 1-disable)
DigitalOut  q_ena_mppt          (PTC0,0);     //Enable MPPT Control (0- enable 1-disable)
DigitalOut  q_boost_ps          (PTC7,1);     //Boost Power Save (1- enable 0-disable)
DigitalOut  q_tca9548_reset     (PTC3,1);     //Reset TCA9548 (1- enable 0-disable)
DigitalOut  power_36khz         (PTD0,0);     //Power enable pic12f - 36khz (0- enable 1-disable)


// ********************************************************************
// ********************************************************************
//DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
//ENTRADAS DIGITAIS (normal input)
DigitalIn   i_enc_dir_rig       (PTB8);     //Encoder Right Direction
DigitalIn   i_enc_dir_lef       (PTB9);     //Encoder Left Direction
DigitalIn   i_micro_sd_det      (PTC16);    //MICRO SD Card Detect
DigitalIn   i_mppt_fail         (PTE5);     //Fail MPPT Signal
DigitalIn   i_usb_volt          (PTB10);    //USB Voltage detect
DigitalIn   i_sup_cap_est       (PTB19);    //Supercap State Charger
DigitalIn   i_li_ion_est        (PTB18);    //Li-ion State Charger


// ********************************************************************
//ENTRADAS DIGITAIS (external interrupt)
InterruptIn i_int_mpu9250       (PTA15);    //Interrupt MPU9250
InterruptIn i_int_isl29125      (PTA16);    //Interrupt ISL29125 Color S.
InterruptIn i_mic_f_l           (PTD7);     //Interrupt Comp Micro F L
InterruptIn i_mic_f_r           (PTD6);     //Interrupt Comp Micro F R
InterruptIn i_mic_r_c           (PTD5);     //Interrupt Comp Micro R C


// ********************************************************************
//ENTRADAS ANALOGICAS
AnalogIn    a_enc_rig           (PTC2);     //Encoder Left Output_AS_MR
AnalogIn    a_enc_lef           (PTC1);     //Encoder Right Output_AS_ML
AnalogIn    a_mic_f_l           (PTB0);     //Analog microphone F L
AnalogIn    a_mic_f_r           (PTB1);     //Analog microphone F R
AnalogIn    a_mic_r_c           (PTB2);     //Analog microphone R C
AnalogIn    a_temp_bat          (PTB3);     //Temperature Battery


// ********************************************************************

//PWM OR DIGITAL OUTPUT NORMAL
//DigitalOut    q_led_whi         (PTE29);    //Led white pwm
DigitalOut    q_led_red_fro     (PTA4);     //Led Red Front
DigitalOut    q_led_gre_fro     (PTA5);     //Led Green Front
DigitalOut    q_led_blu_fro     (PTA12);    //Led Blue Front
DigitalOut    q_led_red_rea     (PTD4);     //Led Red Rear
DigitalOut    q_led_gre_rea     (PTA1);     //Led Green Rear
DigitalOut    q_led_blu_rea     (PTA2);     //Led Blue Rear


//SAIDAS DIGITAIS (pwm)
PwmOut      pwm_mot_rig         (PTE20);    //PWM Enable Motor Right
PwmOut      pwm_mot_lef         (PTE31);    //PWM Enable Motor Left
PwmOut      pwm_buzzer          (PTE21);    //Buzzer PWM
PwmOut      pwm_led_whi         (PTE29);    //Led white pwm

// ********************************************************************
//SAIDAS ANALOGICAS
AnalogOut   dac_comp_mic        (PTE30);        //Dac_Comparator MIC




/**
 * Selects the wich infrared to comunicate.
 *
 * @param ch - Infrared to read (1..5)
 */
void tca9548_select_ch(char ch)
{
    char ch_f[1];
    char addr=0xE0;

    if(ch==0)
        ch_f[0]=1;

    if(ch>=1)
        ch_f[0]=1<<ch;

mutex_i2c0.lock();
    i2c.write(addr,ch_f,1);
    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 the distance from of the chosen infrared.
 *
 * @param ch - Infrared to read (0,1..5)
 *
 * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
 */
float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
{
    tca9548_select_ch(ch);
    VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue);    // read prox value on demand
    float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
    return aux;
    //return ProxiValue;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////     MOTOR       ///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

// Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
// consultar pag 39 e 95

/**
 * Sets speed and direction of the left motor.
 *
 * @param Dir - Direction of movement, 0 for back, or 1 for fron
 * @param Speed - Percentage of speed of the motor (1..100)
 *
 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
 *  at different speeds and see if it makes a straigth line
 */
void leftMotor(short int Dir,short int Speed)
{
    float Duty;

    if(Dir==1) {
        q_pha_mot_lef=0;            //Andar em frente
        if(Speed>1000)                   //limite de segurança
            Speed=1000;
        if(Speed>0) {
            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
            q_sleep_mot_lef=1;          //Nano Sleep Motor Left
            pwm_mot_lef.pulsewidth_us(Duty*5);
        } else {
            q_sleep_mot_lef=0;
        }
    }
    if(Dir==0) {
        q_pha_mot_lef=1;            //Andar para tras

        if(Speed>1000)                   //limite de segurança
            Speed=1000;
        if(Speed>0) {
            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
            q_sleep_mot_lef=1;          //Nano Sleep Motor Left
            pwm_mot_lef.pulsewidth_us(Duty*5);
        } else {
            q_sleep_mot_lef=0;
        }
    }
}


/**
 * Sets speed and direction of the right motor.
 *
 * @param Dir - Direction of movement, 0 for back, or 1 for fron
 * @param Speed - Percentage of speed of the motor (1..100)
 *
 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
 *  at different speeds and see if it makes a straigth line
 */
void rightMotor(short int Dir,short int Speed)
{
    float Duty;

    if(Dir==1) {
        q_pha_mot_rig=0;            //Andar em frente

        if(Speed>1000)                   //limite de segurança
            Speed=1000;
        if(Speed>0) {
            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
            q_sleep_mot_rig=1;          //Nano Sleep Motor Right
            pwm_mot_rig.pulsewidth_us(Duty*5);
        } else {
            q_sleep_mot_rig=0;
        }
    }
    if(Dir==0) {
        q_pha_mot_rig=1;            //Andar para tras


        if(Speed>1000)                   //limite de segurança
            Speed=1000;
        if(Speed>0) {
            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
            q_sleep_mot_rig=1;          //Nano Sleep Motor Right
            pwm_mot_rig.pulsewidth_us(Duty*5);
        } else {
            q_sleep_mot_rig=0;
        }
    }
}


///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////     BATTERY     ///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

/**
 * Reads adc of the battery.
 *
 * @param addr - Address to read
 * @return The voltage of the baterry
 */
long int read16_mcp3424(char addr)
{
    char data[4];
    mutex_i2c1.lock();
    i2c1.read(addr,data,3);
    mutex_i2c1.unlock();
    return(((data[0]&127)*256)+data[1]);
}

/**
 * Reads adc of the battery.
 *
 * @param n_bits - Resolution of measure
 * @param ch - Chose value to read, if voltage or current of solar or batery
 * @param gain -
 * @param addr - Address to write to
 */
void write_mcp3424(int n_bits, int  ch, int gain, char  addr)  //chanel 1-4    write -> 0xD0
{

    int chanel_end=(ch-1)<<5; //shift left
    char n_bits_end=0;

    if(n_bits==12) {
        n_bits_end=0;
    } else if(n_bits==14) {
        n_bits_end=1;
    } else if(n_bits==16) {
        n_bits_end=2;
    } else {
        n_bits_end=3;
    }
    n_bits_end=n_bits_end<<2; //shift left

    char data[1];
    data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
    mutex_i2c1.lock();
    i2c1.write(addr,data,1);
    mutex_i2c1.unlock();
}


/**
 * Reads adc of the battery.
 *
 * @return The voltage of the batery
 */
float value_of_Batery()
{
    float   R1=75000.0;
    float   R2=39200.0;
    float   R3=178000.0;
    float   Gain=1.0;
    write_mcp3424(16,3,1,0xd8);
    float cha3_v2=read16_mcp3424(0xd9); //read  voltage
    float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
    float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
    Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;

    return Vin_b_v_battery;
}

///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////      Sonar     ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
//      Commands of operation with ultrasonic module

//    WRITE OPTION:
//        ENABLE DC DC CONVERTER          - 0x0C;
//        DISABLE DC DC CONVERTER         - 0x0B;
//        START MEASURE LEFT SENSOR       - 0x0A;
//        START MEASURE FRONT SENSOR      - 0x09;
//        START MEASURE RIGHT SENSOR      - 0x08;
//        SENSORS ALWAYS MEASURE ON       - 0x07;
//        SENSORS ALWAYS MEASURE OFF      - 0x06;

// READ OPTION:
//        GET MEASURE OF LEFT SENSOR          - 0x05;
//        GET MEASURE OF FRONT SENSOR         - 0x04;
//        GET MEASURE OF IGHT SENSOR          - 0x03;
//        GET STATUS SENSORS ALWAYS MEASURE   - 0x02;
//        GET STATUS DC DC CONVERTER          - 0x01;

void enable_dc_dc_boost()
{
    char data[1];
    data[0]= 0x0C;
    mutex_i2c1.lock();
    i2c1.write(0x30,data,1);
    mutex_i2c1.unlock();
}


void measure_always_on()  // left, front, right
{
    char data[1];
    data[0]= 0x07;
    mutex_i2c1.lock();
    i2c1.write(0x30,data,1);
    mutex_i2c1.unlock();
}


/**
 * Returns left sensor value
 */
static unsigned int get_distance_left_sensor()
{
      
    static char data_r[3];
    static unsigned int aux;
    
    data_r[0]= 0x05;
    mutex_i2c1.lock();
    i2c1.write(0x30,data_r,1);
    i2c1.read(0x31,data_r,2);
    mutex_i2c1.unlock();

    aux=(data_r[0]*256)+data_r[1];

    return aux;
}


/**
 * Returns front sensor value
 */
static unsigned int get_distance_front_sensor()
{

    static char data_r[3];
    static unsigned int aux;

    data_r[0]= 0x04;
    
    mutex_i2c1.lock();
    i2c1.write(0x30,data_r,1);
    i2c1.read(0x31,data_r,2);
    mutex_i2c1.unlock();

    aux=(data_r[0]*256)+data_r[1];

    return aux;

}


/**
 * Returns right sensor value
 */
static unsigned int get_distance_right_sensor()
{

    static char data_r[3];
    static unsigned int aux;

    data_r[0]= 0x03;

    mutex_i2c1.lock();
    i2c1.write(0x30,data_r,1);
    i2c1.read(0x31,data_r,2,0);
    mutex_i2c1.unlock();

    aux=(data_r[0]*256)+data_r[1];

    return aux;

}


///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////     RF COMMUNICATION      ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

/**
 * 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];


/**
 * Wireless control of motors using nrf24l01 module and FRDM-KL25Z board(robot is working as receiver).
 */
void robot_RC()
{
   if (radio.available() ) {

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



/**
 * 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

    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


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

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


///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////      MISC.      ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////


/**
 * Initializes the necessary robot pins
 */
void init_robot_pins()
{

    //SAIDAS DIGITAIS (normal)
    //q_pha_mot_rig=0;            //Phase Motor Right
    //q_sleep_mot_rig=0;          //Nano Sleep Motor Right
    //q_pha_mot_lef=0;            //Phase Motor Left
    //q_sleep_mot_lef=0;          //Nano Sleep Motor Left
    //q_pow_ena_i2c_p=0;          //Power Enable i2c FET P
    //q_pow_ena_mic_p=0;          //Power enable Micro FET P
    //q_pow_as5600_n=1;           //AS5600 Power MOSFET N
    //q_pow_as5600_p=0;           //AS5600 Power MOSFET P
    //q_pow_spi=0;                //SPI Power MOSFET P
    //q_ena_mppt=0;               //Enable MPPT Control
    //q_boost_ps=1;               //Boost Power Save
    //q_tca9548_reset=1;          //Reset TCA9548

    //SAIDAS DIGITAIS (normal)
    q_pha_mot_rig=0;            //Phase Motor Right
    q_sleep_mot_rig=0;          //Nano Sleep Motor Right
    q_pha_mot_lef=0;            //Phase Motor Left
    q_sleep_mot_lef=0;          //Nano Sleep Motor Left

    q_pow_ena_i2c_p=0;         //Power Enable i2c FET P
    q_pow_ena_mic_p=0;          //Power enable Micro FET P
    q_pow_as5600_p=0;           //AS5600 Power MOSFET P
    // q_pow_spi=0;                //SPI Power MOSFET P
    q_pow_as5600_n=1;           //AS5600 Power MOSFET N


    q_ena_mppt=0;               //Enable MPPT Control
    q_boost_ps=1;               //Boost Power Save
    q_tca9548_reset=1;          //Reset TCA9548

    //Leds caso seja saida digital:
    q_led_red_fro=1;          //Led Red Front (led off)
    q_led_gre_fro=1;          //Led Green Front (led off)
    q_led_blu_fro=1;          //Led Blue Front (led off)
    q_led_red_rea=1;          //Led Red Rear (led off)
    q_led_gre_rea=1;          //Led Green Rear (led off)
    q_led_blu_rea=1;          //Led Blue Rear (led off)r


//********************************************************************
    //SAIDAS DIGITAIS (pwm)
    //PWM Enable Motor Right
    pwm_mot_rig.period_us(500);
    pwm_mot_rig.pulsewidth_us(0);

    //PWM Enable Motor Left
    pwm_mot_lef.period_us(500);
    pwm_mot_lef.pulsewidth_us(0);

    //Buzzer PWM
    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(int channel)
{   
    pc.printf("Battery level: \n\r");
    init_nRF(channel);
    init_robot_pins();
    enable_dc_dc_boost();
    wait_ms(100); //wait for read wait(>=150ms);
    enable_dc_dc_boost();
    measure_always_on();
    wait_ms(100); //wait for read wait(>=150ms);
   
    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));

}