robot.h

00001 /** @file */
00002 //AUTOR: Fernando Pais
00003 //MAIL:  ferpais2508@gmail.com
00004 //DATA: 6/6/2016
00005 // VERSÃO 6.4.0
00006 //
00007 //Alterações: Problema de compatibilidade entre encoder e infravermelho resolvido
00008 //            Odometria actualizada automaticamente
00009 //            Valor da bateria verificado na inicialização
00010 //            Motores movem-se de 0 a 1000 para melhor difrenciação
00011 //
00012 
00013 //#include "mbed.h"
00014 //#include "init.h"
00015 //#define _USE_MATH_DEFINES
00016 # define M_PI           3.14159265358979323846  /* pi */
00017 #include <math.h>
00018 #include <string.h>
00019 #include "VCNL40x0.h"
00020 
00021 void Odometria();
00022 
00023 // classes adicionais
00024 
00025 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
00026 Timeout timeout;
00027 
00028 Serial pc(PTE0,PTE1);
00029 I2C i2c(PTC9,PTC8);
00030 I2C i2c1(PTC11,PTC10);
00031 
00032 // Variables needed by the lib
00033 unsigned int  ProxiValue=0;
00034 short int prev_R=0;
00035 short int prev_L=0;
00036 long int total_R=0;
00037 long int total_L=0;
00038 long int ticks2d=0;
00039 long int ticks2e=0;
00040 float X=20;
00041 float Y=20;
00042 float AtractX = 0;
00043 float AtractY = 0;
00044 float theta=0;
00045 int sensor_left=0;
00046 int sensor_front=0;
00047 int sensor_right=0;
00048 short int flag=0;
00049 int IRobot=0;
00050 int JRobot=0;
00051 
00052 //SAIDAS DIGITAIS (normal)
00053 DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
00054 DigitalOut  q_sleep_mot_rig     (PTE3,0);     //Nano Sleep Motor Right
00055 DigitalOut  q_pha_mot_lef       (PTA17,0);    //Phase Motor Left
00056 DigitalOut  q_sleep_mot_lef     (PTB11,0);    //Nano Sleep Motor Left
00057 DigitalOut  q_pow_ena_i2c_p     (PTE2,0);     //Power Enable i2c FET P (0- enable 1-disable)
00058 DigitalOut  q_pow_ena_mic_p     (PTA14,0);    //Power enable Micro FET P (0- enable 1-disable)
00059 DigitalOut  q_pow_as5600_n      (PTC6,1);     //AS5600 Power MOSFET N (1- enable 0-disable)
00060 DigitalOut  q_pow_as5600_p      (PTC5,0);     //AS5600 Power MOSFET P (0- enable 1-disable)
00061 DigitalOut  q_pow_spi           (PTC4,0);     //SPI Power MOSFET P (0- enable 1-disable)
00062 DigitalOut  q_ena_mppt          (PTC0,0);     //Enable MPPT Control (0- enable 1-disable)
00063 DigitalOut  q_boost_ps          (PTC7,1);     //Boost Power Save (1- enable 0-disable)
00064 DigitalOut  q_tca9548_reset     (PTC3,1);     //Reset TCA9548 (1- enable 0-disable)
00065 DigitalOut  power_36khz         (PTD0,0);     //Power enable pic12f - 36khz (0- enable 1-disable)
00066 
00067 
00068 // ********************************************************************
00069 // ********************************************************************
00070 //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
00071 //ENTRADAS DIGITAIS (normal input)
00072 DigitalIn   i_enc_dir_rig       (PTB8);     //Encoder Right Direction
00073 DigitalIn   i_enc_dir_lef       (PTB9);     //Encoder Left Direction
00074 DigitalIn   i_micro_sd_det      (PTC16);    //MICRO SD Card Detect
00075 DigitalIn   i_mppt_fail         (PTE5);     //Fail MPPT Signal
00076 DigitalIn   i_usb_volt          (PTB10);    //USB Voltage detect
00077 DigitalIn   i_sup_cap_est       (PTB19);    //Supercap State Charger
00078 DigitalIn   i_li_ion_est        (PTB18);    //Li-ion State Charger
00079 
00080 
00081 // ********************************************************************
00082 //ENTRADAS DIGITAIS (external interrupt)
00083 InterruptIn i_int_mpu9250       (PTA15);    //Interrupt MPU9250
00084 InterruptIn i_int_isl29125      (PTA16);    //Interrupt ISL29125 Color S.
00085 InterruptIn i_mic_f_l           (PTD7);     //Interrupt Comp Micro F L
00086 InterruptIn i_mic_f_r           (PTD6);     //Interrupt Comp Micro F R
00087 InterruptIn i_mic_r_c           (PTD5);     //Interrupt Comp Micro R C
00088 
00089 
00090 // ********************************************************************
00091 //ENTRADAS ANALOGICAS
00092 AnalogIn    a_enc_rig           (PTC2);     //Encoder Left Output_AS_MR
00093 AnalogIn    a_enc_lef           (PTC1);     //Encoder Right Output_AS_ML
00094 AnalogIn    a_mic_f_l           (PTB0);     //Analog microphone F L
00095 AnalogIn    a_mic_f_r           (PTB1);     //Analog microphone F R
00096 AnalogIn    a_mic_r_c           (PTB2);     //Analog microphone R C
00097 AnalogIn    a_temp_bat          (PTB3);     //Temperature Battery
00098 
00099 
00100 // ********************************************************************
00101 
00102 //PWM OR DIGITAL OUTPUT NORMAL
00103 //DigitalOut    q_led_whi         (PTE29);    //Led white pwm
00104 DigitalOut    q_led_red_fro     (PTA4);     //Led Red Front
00105 DigitalOut    q_led_gre_fro     (PTA5);     //Led Green Front
00106 DigitalOut    q_led_blu_fro     (PTA12);    //Led Blue Front
00107 DigitalOut    q_led_red_rea     (PTD4);     //Led Red Rear
00108 DigitalOut    q_led_gre_rea     (PTA1);     //Led Green Rear
00109 DigitalOut    q_led_blu_rea     (PTA2);     //Led Blue Rear
00110 
00111 
00112 //SAIDAS DIGITAIS (pwm)
00113 PwmOut      pwm_mot_rig         (PTE20);    //PWM Enable Motor Right
00114 PwmOut      pwm_mot_lef         (PTE31);    //PWM Enable Motor Left
00115 PwmOut      pwm_buzzer          (PTE21);    //Buzzer PWM
00116 PwmOut      pwm_led_whi         (PTE29);    //Led white pwm
00117 
00118 // ********************************************************************
00119 //SAIDAS ANALOGICAS
00120 AnalogOut   dac_comp_mic        (PTE30);        //Dac_Comparator MIC
00121 
00122 
00123 /* Powers up all the VCNL4020. */
00124 void init_Infrared()
00125 {
00126     VCNL40x0_Device.SetCurrent (20);     // Set current to 200mA
00127 }
00128 
00129 /**
00130  * Selects the wich infrared to comunicate.
00131  *
00132  * @param ch - Infrared to read (1..5)
00133  */
00134 void tca9548_select_ch(char ch)
00135 {
00136     char ch_f[1];
00137     char addr=0xE0;
00138 
00139     if(ch==0)
00140         ch_f[0]=1;
00141 
00142     if(ch>=1)
00143         ch_f[0]=1<<ch;
00144 
00145     i2c.start();
00146     i2c.write(addr,ch_f,1);
00147     i2c.stop();
00148 }
00149 
00150 
00151 /**
00152  * Get ADC value of the chosen infrared.
00153  *
00154  * @param ch - Infrared to read (1..5)
00155  *
00156  * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
00157  */
00158 long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
00159 {
00160     tca9548_select_ch(ch);
00161     VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue);    // read prox value on demand
00162 
00163     return ProxiValue;
00164 }
00165 
00166 ///////////////////////////////////////////////////////////////////////////////////////////////
00167 ///////////////////////////////////     MOTOR       ///////////////////////////////////////////
00168 ///////////////////////////////////////////////////////////////////////////////////////////////
00169 
00170 // Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
00171 // consultar pag 39 e 95
00172 
00173 /**
00174  * Sets speed and direction of the left motor.
00175  *
00176  * @param Dir - Direction of movement, 0 for back, or 1 for fron
00177  * @param Speed - Percentage of speed of the motor (1..100)
00178  *
00179  * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
00180  *  at different speeds and see if it makes a straigth line
00181  */
00182 void leftMotor(short int Dir,short int Speed)
00183 {
00184     float Duty;
00185 
00186     if(Dir==1) {
00187         q_pha_mot_lef=0;            //Andar em frente
00188         if(Speed>1000)                   //limite de segurança
00189             Speed=1000;
00190         if(Speed>0) {
00191             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00192             q_sleep_mot_lef=1;          //Nano Sleep Motor Left
00193             pwm_mot_lef.pulsewidth_us(Duty*5);
00194         } else {
00195             q_sleep_mot_lef=0;
00196         }
00197     }
00198     if(Dir==0) {
00199         q_pha_mot_lef=1;            //Andar para tras
00200 
00201         if(Speed>1000)                   //limite de segurança
00202             Speed=1000;
00203         if(Speed>0) {
00204             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00205             q_sleep_mot_lef=1;          //Nano Sleep Motor Left
00206             pwm_mot_lef.pulsewidth_us(Duty*5);
00207         } else {
00208             q_sleep_mot_lef=0;
00209         }
00210     }
00211 }
00212 
00213 
00214 /**
00215  * Sets speed and direction of the right motor.
00216  *
00217  * @param Dir - Direction of movement, 0 for back, or 1 for fron
00218  * @param Speed - Percentage of speed of the motor (1..100)
00219  *
00220  * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
00221  *  at different speeds and see if it makes a straigth line
00222  */
00223 void rightMotor(short int Dir,short int Speed)
00224 {
00225     float Duty;
00226 
00227     if(Dir==1) {
00228         q_pha_mot_rig=0;            //Andar em frente
00229 
00230         if(Speed>1000)                   //limite de segurança
00231             Speed=1000;
00232         if(Speed>0) {
00233             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00234             q_sleep_mot_rig=1;          //Nano Sleep Motor Right
00235             pwm_mot_rig.pulsewidth_us(Duty*5);
00236         } else {
00237             q_sleep_mot_rig=0;
00238         }
00239     }
00240     if(Dir==0) {
00241         q_pha_mot_rig=1;            //Andar para tras
00242 
00243 
00244         if(Speed>1000)                   //limite de segurança
00245             Speed=1000;
00246         if(Speed>0) {
00247             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00248             q_sleep_mot_rig=1;          //Nano Sleep Motor Right
00249             pwm_mot_rig.pulsewidth_us(Duty*5);
00250         } else {
00251             q_sleep_mot_rig=0;
00252         }
00253     }
00254 }
00255 
00256 
00257 ///////////////////////////////////////////////////////////////////////////////////////////////
00258 ///////////////////////////////////     ENCODER     ///////////////////////////////////////////
00259 ///////////////////////////////////////////////////////////////////////////////////////////////
00260 
00261 /**
00262  * Reads Position of left magnetic encoder.
00263  *
00264  * @return The absolute position of the left wheel encoder (0..4095)
00265  */
00266 long int read_L_encoder()
00267 {
00268 
00269     char data_r_2[5];
00270 
00271     i2c.start();
00272     i2c.write(0x6C);
00273     i2c.write(0x0C);
00274     i2c.read(0x6D,data_r_2,4,0);
00275     i2c.stop();
00276 
00277     short int val1=data_r_2[0];
00278     short int val2=data_r_2[1];
00279     val1=(val1&0xf)*256;
00280     long int final=(val2+val1);
00281 
00282     return  final;
00283 }
00284 
00285 
00286 /**
00287  * Reads Position of right magnetic encoder.
00288  *
00289  * @return The absolute position of the right wheel encoder (0..4095)
00290  */
00291 long int read_R_encoder()
00292 {
00293 
00294     char data_r_2[5];
00295 
00296     i2c1.start();
00297     i2c1.write(0x6C);
00298     i2c1.write(0x0C);
00299     i2c1.read(0x6D,data_r_2,4,0);
00300     i2c1.stop();
00301 
00302     short int val1=data_r_2[0];
00303     short int val2=data_r_2[1];
00304     val1=(val1&0xf)*256;
00305     long int final=(val2+val1);
00306 
00307     return  final;
00308 }
00309 
00310 
00311 /**
00312  * Calculates and returns the value of the  right "incremental" encoder.
00313  *
00314  * @return The value of "tics" of the right encoder since it was initialized
00315  */
00316 long int incremental_R_encoder()
00317 {
00318     short int next_R=read_R_encoder(); // Reads curent value of the encoder
00319     short int dif=next_R-prev_R;       // Calculates the diference from last reading
00320 
00321     if(dif>3000) {                     // Going back and pass zero
00322         total_R=total_R-4096+dif;
00323     }
00324     if(dif<3000&&dif>0) {              // Going front
00325         total_R=total_R+dif;
00326     }
00327     if(dif<-3000) {                    // Going front and pass zero
00328         total_R=total_R+4096+dif;
00329     }
00330     if(dif>-3000&&dif<0) {             // going back
00331         total_R=total_R+dif;
00332     }
00333     prev_R=next_R;                     // Sets last reading for next iteration
00334 
00335     return  total_R;
00336 }
00337 
00338 
00339 /**
00340  * Calculates and returns the value of the  left "incremental" encoder.
00341  *
00342  * @return The value of "tics" of the left encoder since it was initialized
00343  */
00344 long int incremental_L_encoder()
00345 {
00346     short int next_L=read_L_encoder(); // Reads curent value of the encoder
00347     short int dif=-next_L+prev_L;      // Calculates the diference from last reading
00348 
00349     if(dif>3000) {                     // Going back and pass zero
00350         total_L=total_L-4096+dif;
00351     }
00352     if(dif<3000&&dif>0) {              // Going front
00353         total_L=total_L+dif;
00354     }
00355     if(dif<-3000) {                    // Going front and pass zero
00356         total_L=total_L+4096+dif;
00357     }
00358     if(dif>-3000&&dif<0) {             // going back
00359         total_L=total_L+dif;
00360     }
00361     prev_L=next_L;                     // Sets last reading for next iteration
00362 
00363     return  total_L;
00364 }
00365 
00366 
00367 /**
00368  * Calculate the value of both encoder "incremental" every 10 ms.
00369  */
00370 void timer_event()  //10ms interrupt
00371 {
00372     timeout.attach(&timer_event,0.01);
00373     if(flag==0) {
00374         incremental_R_encoder();
00375         incremental_L_encoder();
00376     }
00377     Odometria();
00378 
00379     return;
00380 }
00381 
00382 
00383 /**
00384  * Set the initial position for the "incremental" enconder and "starts" them.
00385  */
00386 void initEncoders()
00387 {
00388     prev_R=read_R_encoder();
00389     prev_L=read_L_encoder();
00390     timeout.attach(&timer_event,0.01);
00391 }
00392 
00393 
00394 /**
00395  * Returns to the user the value of the right "incremental" encoder.
00396  *
00397  * @return The value of "tics" of the right encoder since it was initialized
00398  */
00399 long int R_encoder()
00400 {
00401     wait(0.0001);
00402 
00403     return total_R;
00404 }
00405 
00406 /**
00407  * Returns to the user the value of the right "incremental" encoder.
00408  *
00409  * @return The value of "tics" of the right encoder since it was initialized
00410  */
00411 long int L_encoder()
00412 {
00413     wait(0.0001);
00414 
00415     return total_L;
00416 }
00417 
00418 
00419 ///////////////////////////////////////////////////////////////////////////////////////////////
00420 ///////////////////////////////////     BATTERY     ///////////////////////////////////////////
00421 ///////////////////////////////////////////////////////////////////////////////////////////////
00422 
00423 /**
00424  * Reads adc of the battery.
00425  *
00426  * @param addr - Address to read
00427  * @return The voltage of the batery
00428  */
00429 long int read16_mcp3424(char addr)
00430 {
00431     char data[4];
00432     i2c1.start();
00433     i2c1.read(addr,data,3);
00434     i2c1.stop();
00435 
00436     return(((data[0]&127)*256)+data[1]);
00437 }
00438 
00439 /**
00440  * Reads adc of the battery.
00441  *
00442  * @param n_bits - Resolution of measure
00443  * @param ch - Chose value to read, if voltage or current of solar or batery
00444  * @param gain -
00445  * @param addr - Address to write to
00446  */
00447 void write_mcp3424(int n_bits, int  ch, int gain, char  addr)  //chanel 1-4    write -> 0xD0
00448 {
00449 
00450     int chanel_end=(ch-1)<<5; //shift left
00451     char n_bits_end=0;
00452 
00453     if(n_bits==12) {
00454         n_bits_end=0;
00455     } else if(n_bits==14) {
00456         n_bits_end=1;
00457     } else if(n_bits==16) {
00458         n_bits_end=2;
00459     } else {
00460         n_bits_end=3;
00461     }
00462     n_bits_end=n_bits_end<<2; //shift left
00463 
00464     char data[1];
00465     data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
00466     i2c1.start();
00467     i2c1.write(addr,data,1);
00468     i2c1.stop();
00469 }
00470 
00471 
00472 /**
00473  * Reads adc of the battery.
00474  *
00475  * @return The voltage of the batery
00476  */
00477 float value_of_Batery()
00478 {
00479     float   R1=75000.0;
00480     float   R2=39200.0;
00481     float   R3=178000.0;
00482     float   Gain=1.0;
00483     write_mcp3424(16,3,1,0xd8);
00484     float cha3_v2=read16_mcp3424(0xd9); //read  voltage
00485     float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
00486     float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
00487     Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
00488 
00489     return Vin_b_v_battery;
00490 }
00491 
00492 
00493 ///////////////////////////////////////////////////////////////////////////////////////////////
00494 //////////////////////////////////      Sonar     ////////////////////////////////////////////
00495 ///////////////////////////////////////////////////////////////////////////////////////////////
00496 //      Commands of operation with ultrasonic module
00497 
00498 //    WRITE OPTION:
00499 //        ENABLE DC DC CONVERTER          - 0x0C;
00500 //        DISABLE DC DC CONVERTER         - 0x0B;
00501 //        START MEASURE LEFT SENSOR       - 0x0A;
00502 //        START MEASURE FRONT SENSOR      - 0x09;
00503 //        START MEASURE RIGHT SENSOR      - 0x08;
00504 //        SENSORS ALWAYS MEASURE ON       - 0x07;
00505 //        SENSORS ALWAYS MEASURE OFF      - 0x06;
00506 
00507 // READ OPTION:
00508 //        GET MEASURE OF LEFT SENSOR          - 0x05;
00509 //        GET MEASURE OF FRONT SENSOR         - 0x04;
00510 //        GET MEASURE OF IGHT SENSOR          - 0x03;
00511 //        GET STATUS SENSORS ALWAYS MEASURE   - 0x02;
00512 //        GET STATUS DC DC CONVERTER          - 0x01;
00513 
00514 void enable_dc_dc_boost()
00515 {
00516     char data[1];
00517     data[0]= 0x0C;
00518     wait_ms(1);
00519     i2c1.start();
00520     i2c1.write(0x30,data,1);
00521     i2c1.stop();
00522     i2c1.start();
00523     i2c1.write(0x30,data,1);
00524     i2c1.stop();
00525 }
00526 
00527 
00528 void disable_dc_dc_boost()
00529 {
00530     char data[1];
00531     data[0]= 0x0B;
00532     wait_ms(1);
00533     i2c1.start();
00534     i2c1.write(0x30,data,1);
00535     i2c1.stop();
00536 }
00537 
00538 
00539 void start_read_left_sensor()
00540 {
00541     char data[1];
00542     data[0]= 0x0A;
00543     wait_ms(1);
00544     i2c1.start();
00545     i2c1.write(0x30,data,1);
00546     i2c1.stop();
00547 }
00548 
00549 
00550 void start_read_front_sensor()
00551 {
00552     char data[1];
00553     data[0]= 0x09;
00554     wait_ms(1);
00555     i2c1.start();
00556     i2c1.write(0x30,data,1);
00557     i2c1.stop();
00558 }
00559 
00560 
00561 void start_read_right_sensor()
00562 {
00563     char data[1];
00564     data[0]= 0x08;
00565     wait_ms(1);
00566     i2c1.start();
00567     i2c1.write(0x30,data,1);
00568     i2c1.stop();
00569 }
00570 
00571 
00572 void measure_always_on()  // left, front, right
00573 {
00574     char data[1];
00575     data[0]= 0x07;
00576     wait_ms(1);
00577     i2c1.start();
00578     i2c1.write(0x30,data,1);
00579     i2c1.stop();
00580 }
00581 
00582 
00583 void measure_always_off()
00584 {
00585     char data[1];
00586     data[0]= 0x06;
00587     wait_ms(1);
00588     i2c1.start();
00589     i2c1.write(0x30,data,1);
00590     i2c1.stop();
00591 }
00592 
00593 /**
00594  * Returns left sensor value
00595  */
00596 static unsigned int get_distance_left_sensor()
00597 {
00598 
00599     static char data_r[3];
00600     static unsigned int aux;
00601     flag=1;
00602 
00603     data_r[0]= 0x05;
00604     wait_ms(1);
00605     i2c1.start();
00606     i2c1.write(0x30,data_r,1);
00607     i2c1.stop();
00608     wait_ms(10);
00609     i2c1.start();
00610     i2c1.read(0x31,data_r,2,0);
00611     i2c1.stop();
00612 
00613     aux=(data_r[0]*256)+data_r[1];
00614     flag=0;
00615     return aux;
00616     // sensor_left=aux;
00617     // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
00618 
00619 }
00620 
00621 
00622 /**
00623  * Returns front sensor value
00624  */
00625 static unsigned int get_distance_front_sensor()
00626 {
00627 
00628     static char data_r[3];
00629     static unsigned int aux;
00630     flag=1;
00631     data_r[0]= 0x04;
00632     wait_ms(1);
00633     i2c1.start();
00634     i2c1.write(0x30,data_r,1);
00635     i2c1.stop();
00636     wait_ms(10);
00637     i2c1.start();
00638     i2c1.read(0x31,data_r,2,0);
00639     i2c1.stop();
00640 
00641     aux=(data_r[0]*256)+data_r[1];
00642     flag=0;
00643     return aux;
00644     // sensor_front=aux;
00645     // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
00646 
00647 }
00648 
00649 
00650 /**
00651  * Returns right sensor value
00652  */
00653 static unsigned int get_distance_right_sensor()
00654 {
00655 
00656     static char data_r[3];
00657     static unsigned int aux;
00658     flag=1;
00659 
00660     data_r[0]= 0x03;
00661     wait_ms(1);
00662     i2c1.start();
00663     i2c1.write(0x30,data_r,1);
00664     i2c1.stop();
00665     wait_ms(10);
00666     i2c1.start();
00667     i2c1.read(0x31,data_r,2,0);
00668     i2c1.stop();
00669 
00670     aux=(data_r[0]*256)+data_r[1];
00671     flag=0;
00672     return aux;
00673     // sensor_right=aux;
00674     // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
00675 
00676 }
00677 
00678 
00679 void get_status_always_measure()
00680 {
00681 
00682     static char data_r[3];
00683     static unsigned int aux;
00684 
00685     data_r[0]= 0x02;
00686     wait_ms(1);
00687     i2c1.start();
00688     i2c1.write(0x30,data_r,1);
00689     i2c1.stop();
00690     wait_ms(10);
00691     i2c1.start();
00692     i2c1.read(0x31,data_r,2,0);
00693     i2c1.stop();
00694 
00695     aux=data_r[0];
00696     pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
00697 
00698 }
00699 
00700 
00701 void get_status_dcdc_converter()
00702 {
00703 
00704     static char data_r[3];
00705     static unsigned int aux;
00706 
00707     data_r[0]= 0x01;
00708     wait_ms(1);
00709     i2c1.start();
00710     i2c1.write(0x30,data_r,1);
00711     i2c1.stop();
00712     wait_ms(10);
00713     i2c1.start();
00714     i2c1.read(0x31,data_r,2,0);
00715     i2c1.stop();
00716 
00717     aux=data_r[0];
00718     pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
00719 
00720 }
00721 
00722 
00723 ///////////////////////////////////////////////////////////////////////////////////////////////
00724 //////////////////////////////////      MISC.      ////////////////////////////////////////////
00725 ///////////////////////////////////////////////////////////////////////////////////////////////
00726 
00727 
00728 /**
00729  * Initializes the necessary robot pins
00730  */
00731 void init_robot_pins()
00732 {
00733 
00734     //SAIDAS DIGITAIS (normal)
00735     //q_pha_mot_rig=0;            //Phase Motor Right
00736     //q_sleep_mot_rig=0;          //Nano Sleep Motor Right
00737     //q_pha_mot_lef=0;            //Phase Motor Left
00738     //q_sleep_mot_lef=0;          //Nano Sleep Motor Left
00739     //q_pow_ena_i2c_p=0;          //Power Enable i2c FET P
00740     //q_pow_ena_mic_p=0;          //Power enable Micro FET P
00741     //q_pow_as5600_n=1;           //AS5600 Power MOSFET N
00742     //q_pow_as5600_p=0;           //AS5600 Power MOSFET P
00743     //q_pow_spi=0;                //SPI Power MOSFET P
00744     //q_ena_mppt=0;               //Enable MPPT Control
00745     //q_boost_ps=1;               //Boost Power Save
00746     //q_tca9548_reset=1;          //Reset TCA9548
00747 
00748     //SAIDAS DIGITAIS (normal)
00749     q_pha_mot_rig=0;            //Phase Motor Right
00750     q_sleep_mot_rig=0;          //Nano Sleep Motor Right
00751     q_pha_mot_lef=0;            //Phase Motor Left
00752     q_sleep_mot_lef=0;          //Nano Sleep Motor Left
00753 
00754     q_pow_ena_i2c_p=0;         //Power Enable i2c FET P
00755     q_pow_ena_mic_p=0;          //Power enable Micro FET P
00756     q_pow_as5600_p=0;           //AS5600 Power MOSFET P
00757     // q_pow_spi=0;                //SPI Power MOSFET P
00758     q_pow_as5600_n=1;           //AS5600 Power MOSFET N
00759 
00760 
00761     q_ena_mppt=0;               //Enable MPPT Control
00762     q_boost_ps=1;               //Boost Power Save
00763     q_tca9548_reset=1;          //Reset TCA9548
00764 
00765     //Leds caso seja saida digital:
00766     q_led_red_fro=1;          //Led Red Front (led off)
00767     q_led_gre_fro=1;          //Led Green Front (led off)
00768     q_led_blu_fro=1;          //Led Blue Front (led off)
00769     q_led_red_rea=1;          //Led Red Rear (led off)
00770     q_led_gre_rea=1;          //Led Green Rear (led off)
00771     q_led_blu_rea=1;          //Led Blue Rear (led off)r
00772 
00773 
00774 //********************************************************************
00775     //SAIDAS DIGITAIS (pwm)
00776     //PWM Enable Motor Right
00777     pwm_mot_rig.period_us(500);
00778     pwm_mot_rig.pulsewidth_us(0);
00779 
00780     //PWM Enable Motor Left
00781     pwm_mot_lef.period_us(500);
00782     pwm_mot_lef.pulsewidth_us(0);
00783 
00784     //Buzzer PWM
00785     pwm_buzzer.period_us(500);
00786     pwm_buzzer.pulsewidth_us(0);
00787 
00788     //LED white
00789     pwm_led_whi.period_us(500);
00790     pwm_led_whi.pulsewidth_us(0);
00791 
00792 }
00793 
00794 
00795 /**
00796  * Initializes all the pins and all the modules necessary
00797  */
00798 void initRobot(void)
00799 {
00800     init_robot_pins();
00801     enable_dc_dc_boost();
00802     //init_Infrared();
00803     //initEncoders();
00804 
00805     enable_dc_dc_boost();
00806     wait_ms(100); //wait for read wait(>=150ms);
00807     measure_always_on();
00808     float value = value_of_Batery();
00809     pc.printf("Initialization Successful \n\r");
00810     pc.printf("Battery level: %f \n\r",value);
00811     if(value < 3.0) {
00812         pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
00813     }
00814 
00815     // float level = value_of_Batery();
00816     // sendValue(int(level*100));
00817 
00818 }
00819 
00820 
00821 ////////////////////////////////////////////////////
00822 
00823 /**
00824  * Updates the position and orientation of the robot based on the data from the encoders
00825  *
00826  * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
00827  * and the distance between them is 7.4
00828  */
00829 void Odometria()
00830 {
00831     long int ticks1d=R_encoder();
00832     long int ticks1e=L_encoder();
00833 
00834     long int D_ticks=ticks1d - ticks2d;
00835     long int E_ticks=ticks1e - ticks2e;
00836 
00837     ticks2d=ticks1d;
00838     ticks2e=ticks1e;
00839 
00840     float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
00841     float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
00842 
00843     float CM=(D_cm + L_cm)/2;
00844 
00845     theta +=(D_cm - L_cm)/7.18;
00846 
00847     theta = atan2(sin(theta), cos(theta));
00848 
00849     // meter entre 0
00850 
00851     X += CM*cos(theta);
00852     Y += CM*sin(theta);
00853 
00854 }