ISR UC / ISR_Mini-explorer

Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.

Dependents:   Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers robot.h Source File

robot.h

Go to the documentation of this file.
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 #include "nRF24L01P.h "
00021 
00022 void Odometria();
00023 
00024 // classes adicionais
00025 nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
00026 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
00027 Timeout timeout;
00028 
00029 Serial pc(PTE0,PTE1);
00030 I2C i2c(PTC9,PTC8);
00031 I2C i2c1(PTC11,PTC10);
00032 
00033 // Variables needed by the lib
00034 unsigned int  ProxiValue=0;
00035 short int prev_R=0;
00036 short int prev_L=0;
00037 long int total_R=0;
00038 long int total_L=0;
00039 long int ticks2d=0;
00040 long int ticks2e=0;
00041 float X=20;
00042 float Y=20;
00043 float AtractX = 0;
00044 float AtractY = 0;
00045 float theta=0;
00046 int sensor_left=0;
00047 int sensor_front=0;
00048 int sensor_right=0;
00049 short int flag=0;
00050 int IRobot=0;
00051 int JRobot=0;
00052 
00053 //SAIDAS DIGITAIS (normal)
00054 DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
00055 DigitalOut  q_sleep_mot_rig     (PTE3,0);     //Nano Sleep Motor Right
00056 DigitalOut  q_pha_mot_lef       (PTA17,0);    //Phase Motor Left
00057 DigitalOut  q_sleep_mot_lef     (PTB11,0);    //Nano Sleep Motor Left
00058 DigitalOut  q_pow_ena_i2c_p     (PTE2,0);     //Power Enable i2c FET P (0- enable 1-disable)
00059 DigitalOut  q_pow_ena_mic_p     (PTA14,0);    //Power enable Micro FET P (0- enable 1-disable)
00060 DigitalOut  q_pow_as5600_n      (PTC6,1);     //AS5600 Power MOSFET N (1- enable 0-disable)
00061 DigitalOut  q_pow_as5600_p      (PTC5,0);     //AS5600 Power MOSFET P (0- enable 1-disable)
00062 DigitalOut  q_pow_spi           (PTC4,0);     //SPI Power MOSFET P (0- enable 1-disable)
00063 DigitalOut  q_ena_mppt          (PTC0,0);     //Enable MPPT Control (0- enable 1-disable)
00064 DigitalOut  q_boost_ps          (PTC7,1);     //Boost Power Save (1- enable 0-disable)
00065 DigitalOut  q_tca9548_reset     (PTC3,1);     //Reset TCA9548 (1- enable 0-disable)
00066 DigitalOut  power_36khz         (PTD0,0);     //Power enable pic12f - 36khz (0- enable 1-disable)
00067 
00068 
00069 // ********************************************************************
00070 // ********************************************************************
00071 //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
00072 //ENTRADAS DIGITAIS (normal input)
00073 DigitalIn   i_enc_dir_rig       (PTB8);     //Encoder Right Direction
00074 DigitalIn   i_enc_dir_lef       (PTB9);     //Encoder Left Direction
00075 DigitalIn   i_micro_sd_det      (PTC16);    //MICRO SD Card Detect
00076 DigitalIn   i_mppt_fail         (PTE5);     //Fail MPPT Signal
00077 DigitalIn   i_usb_volt          (PTB10);    //USB Voltage detect
00078 DigitalIn   i_sup_cap_est       (PTB19);    //Supercap State Charger
00079 DigitalIn   i_li_ion_est        (PTB18);    //Li-ion State Charger
00080 
00081 
00082 // ********************************************************************
00083 //ENTRADAS DIGITAIS (external interrupt)
00084 InterruptIn i_int_mpu9250       (PTA15);    //Interrupt MPU9250
00085 InterruptIn i_int_isl29125      (PTA16);    //Interrupt ISL29125 Color S.
00086 InterruptIn i_mic_f_l           (PTD7);     //Interrupt Comp Micro F L
00087 InterruptIn i_mic_f_r           (PTD6);     //Interrupt Comp Micro F R
00088 InterruptIn i_mic_r_c           (PTD5);     //Interrupt Comp Micro R C
00089 
00090 
00091 // ********************************************************************
00092 //ENTRADAS ANALOGICAS
00093 AnalogIn    a_enc_rig           (PTC2);     //Encoder Left Output_AS_MR
00094 AnalogIn    a_enc_lef           (PTC1);     //Encoder Right Output_AS_ML
00095 AnalogIn    a_mic_f_l           (PTB0);     //Analog microphone F L
00096 AnalogIn    a_mic_f_r           (PTB1);     //Analog microphone F R
00097 AnalogIn    a_mic_r_c           (PTB2);     //Analog microphone R C
00098 AnalogIn    a_temp_bat          (PTB3);     //Temperature Battery
00099 
00100 
00101 // ********************************************************************
00102 
00103 //PWM OR DIGITAL OUTPUT NORMAL
00104 //DigitalOut    q_led_whi         (PTE29);    //Led white pwm
00105 DigitalOut    q_led_red_fro     (PTA4);     //Led Red Front
00106 DigitalOut    q_led_gre_fro     (PTA5);     //Led Green Front
00107 DigitalOut    q_led_blu_fro     (PTA12);    //Led Blue Front
00108 DigitalOut    q_led_red_rea     (PTD4);     //Led Red Rear
00109 DigitalOut    q_led_gre_rea     (PTA1);     //Led Green Rear
00110 DigitalOut    q_led_blu_rea     (PTA2);     //Led Blue Rear
00111 
00112 
00113 //SAIDAS DIGITAIS (pwm)
00114 PwmOut      pwm_mot_rig         (PTE20);    //PWM Enable Motor Right
00115 PwmOut      pwm_mot_lef         (PTE31);    //PWM Enable Motor Left
00116 PwmOut      pwm_buzzer          (PTE21);    //Buzzer PWM
00117 PwmOut      pwm_led_whi         (PTE29);    //Led white pwm
00118 
00119 // ********************************************************************
00120 //SAIDAS ANALOGICAS
00121 AnalogOut   dac_comp_mic        (PTE30);        //Dac_Comparator MIC
00122 
00123 
00124 /* Powers up all the VCNL4020. */
00125 void init_Infrared()
00126 {
00127     VCNL40x0_Device.SetCurrent (20);     // Set current to 200mA
00128 }
00129 
00130 /**
00131  * Selects the wich infrared to comunicate.
00132  *
00133  * @param ch - Infrared to read (1..5)
00134  */
00135 void tca9548_select_ch(char ch)
00136 {
00137     char ch_f[1];
00138     char addr=0xE0;
00139 
00140     if(ch==0)
00141         ch_f[0]=1;
00142 
00143     if(ch>=1)
00144         ch_f[0]=1<<ch;
00145 
00146     i2c.start();
00147     i2c.write(addr,ch_f,1);
00148     i2c.stop();
00149 }
00150 
00151 
00152 /**
00153  * Get ADC value of the chosen infrared.
00154  *
00155  * @param ch - Infrared to read (1..5)
00156  *
00157  * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
00158  */
00159 long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
00160 {
00161     tca9548_select_ch(ch);
00162     VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue);    // read prox value on demand
00163 
00164     return ProxiValue;
00165 }
00166 
00167 ///////////////////////////////////////////////////////////////////////////////////////////////
00168 ///////////////////////////////////     MOTOR       ///////////////////////////////////////////
00169 ///////////////////////////////////////////////////////////////////////////////////////////////
00170 
00171 // Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
00172 // consultar pag 39 e 95
00173 
00174 /**
00175  * Sets speed and direction of the left motor.
00176  *
00177  * @param Dir - Direction of movement, 0 for back, or 1 for fron
00178  * @param Speed - Percentage of speed of the motor (1..100)
00179  *
00180  * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
00181  *  at different speeds and see if it makes a straigth line
00182  */
00183 void leftMotor(short int Dir,short int Speed)
00184 {
00185     float Duty;
00186 
00187     if(Dir==1) {
00188         q_pha_mot_lef=0;            //Andar em frente
00189         if(Speed>1000)                   //limite de segurança
00190             Speed=1000;
00191         if(Speed>0) {
00192             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00193             q_sleep_mot_lef=1;          //Nano Sleep Motor Left
00194             pwm_mot_lef.pulsewidth_us(Duty*5);
00195         } else {
00196             q_sleep_mot_lef=0;
00197         }
00198     }
00199     if(Dir==0) {
00200         q_pha_mot_lef=1;            //Andar para tras
00201 
00202         if(Speed>1000)                   //limite de segurança
00203             Speed=1000;
00204         if(Speed>0) {
00205             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00206             q_sleep_mot_lef=1;          //Nano Sleep Motor Left
00207             pwm_mot_lef.pulsewidth_us(Duty*5);
00208         } else {
00209             q_sleep_mot_lef=0;
00210         }
00211     }
00212 }
00213 
00214 
00215 /**
00216  * Sets speed and direction of the right motor.
00217  *
00218  * @param Dir - Direction of movement, 0 for back, or 1 for fron
00219  * @param Speed - Percentage of speed of the motor (1..100)
00220  *
00221  * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
00222  *  at different speeds and see if it makes a straigth line
00223  */
00224 void rightMotor(short int Dir,short int Speed)
00225 {
00226     float Duty;
00227 
00228     if(Dir==1) {
00229         q_pha_mot_rig=0;            //Andar em frente
00230 
00231         if(Speed>1000)                   //limite de segurança
00232             Speed=1000;
00233         if(Speed>0) {
00234             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00235             q_sleep_mot_rig=1;          //Nano Sleep Motor Right
00236             pwm_mot_rig.pulsewidth_us(Duty*5);
00237         } else {
00238             q_sleep_mot_rig=0;
00239         }
00240     }
00241     if(Dir==0) {
00242         q_pha_mot_rig=1;            //Andar para tras
00243 
00244 
00245         if(Speed>1000)                   //limite de segurança
00246             Speed=1000;
00247         if(Speed>0) {
00248             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00249             q_sleep_mot_rig=1;          //Nano Sleep Motor Right
00250             pwm_mot_rig.pulsewidth_us(Duty*5);
00251         } else {
00252             q_sleep_mot_rig=0;
00253         }
00254     }
00255 }
00256 
00257 
00258 ///////////////////////////////////////////////////////////////////////////////////////////////
00259 ///////////////////////////////////     ENCODER     ///////////////////////////////////////////
00260 ///////////////////////////////////////////////////////////////////////////////////////////////
00261 
00262 /**
00263  * Reads Position of left magnetic encoder.
00264  *
00265  * @return The absolute position of the left wheel encoder (0..4095)
00266  */
00267 long int read_L_encoder()
00268 {
00269 
00270     char data_r_2[5];
00271 
00272     i2c.start();
00273     i2c.write(0x6C);
00274     i2c.write(0x0C);
00275     i2c.read(0x6D,data_r_2,4,0);
00276     i2c.stop();
00277 
00278     short int val1=data_r_2[0];
00279     short int val2=data_r_2[1];
00280     val1=(val1&0xf)*256;
00281     long int final=(val2+val1);
00282 
00283     return  final;
00284 }
00285 
00286 
00287 /**
00288  * Reads Position of right magnetic encoder.
00289  *
00290  * @return The absolute position of the right wheel encoder (0..4095)
00291  */
00292 long int read_R_encoder()
00293 {
00294 
00295     char data_r_2[5];
00296 
00297     i2c1.start();
00298     i2c1.write(0x6C);
00299     i2c1.write(0x0C);
00300     i2c1.read(0x6D,data_r_2,4,0);
00301     i2c1.stop();
00302 
00303     short int val1=data_r_2[0];
00304     short int val2=data_r_2[1];
00305     val1=(val1&0xf)*256;
00306     long int final=(val2+val1);
00307 
00308     return  final;
00309 }
00310 
00311 
00312 /**
00313  * Calculates and returns the value of the  right "incremental" encoder.
00314  *
00315  * @return The value of "tics" of the right encoder since it was initialized
00316  */
00317 long int incremental_R_encoder()
00318 {
00319     short int next_R=read_R_encoder(); // Reads curent value of the encoder
00320     short int dif=next_R-prev_R;       // Calculates the diference from last reading
00321 
00322     if(dif>3000) {                     // Going back and pass zero
00323         total_R=total_R-4096+dif;
00324     }
00325     if(dif<3000&&dif>0) {              // Going front
00326         total_R=total_R+dif;
00327     }
00328     if(dif<-3000) {                    // Going front and pass zero
00329         total_R=total_R+4096+dif;
00330     }
00331     if(dif>-3000&&dif<0) {             // going back
00332         total_R=total_R+dif;
00333     }
00334     prev_R=next_R;                     // Sets last reading for next iteration
00335 
00336     return  total_R;
00337 }
00338 
00339 
00340 /**
00341  * Calculates and returns the value of the  left "incremental" encoder.
00342  *
00343  * @return The value of "tics" of the left encoder since it was initialized
00344  */
00345 long int incremental_L_encoder()
00346 {
00347     short int next_L=read_L_encoder(); // Reads curent value of the encoder
00348     short int dif=-next_L+prev_L;      // Calculates the diference from last reading
00349 
00350     if(dif>3000) {                     // Going back and pass zero
00351         total_L=total_L-4096+dif;
00352     }
00353     if(dif<3000&&dif>0) {              // Going front
00354         total_L=total_L+dif;
00355     }
00356     if(dif<-3000) {                    // Going front and pass zero
00357         total_L=total_L+4096+dif;
00358     }
00359     if(dif>-3000&&dif<0) {             // going back
00360         total_L=total_L+dif;
00361     }
00362     prev_L=next_L;                     // Sets last reading for next iteration
00363 
00364     return  total_L;
00365 }
00366 
00367 
00368 /**
00369  * Calculate the value of both encoder "incremental" every 10 ms.
00370  */
00371 void timer_event()  //10ms interrupt
00372 {
00373     timeout.attach(&timer_event,0.01);
00374     if(flag==0) {
00375         incremental_R_encoder();
00376         incremental_L_encoder();
00377     }
00378     Odometria();
00379 
00380     return;
00381 }
00382 
00383 
00384 /**
00385  * Set the initial position for the "incremental" enconder and "starts" them.
00386  */
00387 void initEncoders()
00388 {
00389     prev_R=read_R_encoder();
00390     prev_L=read_L_encoder();
00391     timeout.attach(&timer_event,0.01);
00392 }
00393 
00394 
00395 /**
00396  * Returns to the user the value of the right "incremental" encoder.
00397  *
00398  * @return The value of "tics" of the right encoder since it was initialized
00399  */
00400 long int R_encoder()
00401 {
00402     wait(0.0001);
00403 
00404     return total_R;
00405 }
00406 
00407 /**
00408  * Returns to the user the value of the right "incremental" encoder.
00409  *
00410  * @return The value of "tics" of the right encoder since it was initialized
00411  */
00412 long int L_encoder()
00413 {
00414     wait(0.0001);
00415 
00416     return total_L;
00417 }
00418 
00419 
00420 ///////////////////////////////////////////////////////////////////////////////////////////////
00421 ///////////////////////////////////     BATTERY     ///////////////////////////////////////////
00422 ///////////////////////////////////////////////////////////////////////////////////////////////
00423 
00424 /**
00425  * Reads adc of the battery.
00426  *
00427  * @param addr - Address to read
00428  * @return The voltage of the batery
00429  */
00430 long int read16_mcp3424(char addr)
00431 {
00432     char data[4];
00433     i2c1.start();
00434     i2c1.read(addr,data,3);
00435     i2c1.stop();
00436 
00437     return(((data[0]&127)*256)+data[1]);
00438 }
00439 
00440 /**
00441  * Reads adc of the battery.
00442  *
00443  * @param n_bits - Resolution of measure
00444  * @param ch - Chose value to read, if voltage or current of solar or batery
00445  * @param gain -
00446  * @param addr - Address to write to
00447  */
00448 void write_mcp3424(int n_bits, int  ch, int gain, char  addr)  //chanel 1-4    write -> 0xD0
00449 {
00450 
00451     int chanel_end=(ch-1)<<5; //shift left
00452     char n_bits_end=0;
00453 
00454     if(n_bits==12) {
00455         n_bits_end=0;
00456     } else if(n_bits==14) {
00457         n_bits_end=1;
00458     } else if(n_bits==16) {
00459         n_bits_end=2;
00460     } else {
00461         n_bits_end=3;
00462     }
00463     n_bits_end=n_bits_end<<2; //shift left
00464 
00465     char data[1];
00466     data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
00467     i2c1.start();
00468     i2c1.write(addr,data,1);
00469     i2c1.stop();
00470 }
00471 
00472 
00473 /**
00474  * Reads adc of the battery.
00475  *
00476  * @return The voltage of the batery
00477  */
00478 float value_of_Batery()
00479 {
00480     float   R1=75000.0;
00481     float   R2=39200.0;
00482     float   R3=178000.0;
00483     float   Gain=1.0;
00484     write_mcp3424(16,3,1,0xd8);
00485     float cha3_v2=read16_mcp3424(0xd9); //read  voltage
00486     float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
00487     float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
00488     Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
00489 
00490     return Vin_b_v_battery;
00491 }
00492 
00493 ///////////////////////////////////////////////////////////////////////////////////////////////
00494 ///////////////////////////////     RF COMUNICATION     ///////////////////////////////////////
00495 ///////////////////////////////////////////////////////////////////////////////////////////////
00496 
00497 
00498 /**
00499  * Initializes the NRF24 module for comunication.
00500  *
00501  * Note: if the module is broken or badly connected this init will cause the code to stop,
00502  *  if all these messages don't appear thats the case
00503  */
00504 void config_init_nrf()
00505 {
00506     my_nrf24l01p.powerUp(); // powers module
00507     my_nrf24l01p.setRfFrequency (2400); // channel 0 (2400-0 ... 2516-116)
00508     my_nrf24l01p.setTransferSize(10);   // number of bytes to be transfer
00509     my_nrf24l01p.setCrcWidth(8);
00510     my_nrf24l01p.enableAutoAcknowledge(NRF24L01P_PIPE_P0); // pipe where data transfer occurs (0..6)
00511     pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
00512     pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
00513     pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
00514     pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
00515     pc.printf( "nRF24L01+ CrC Width    : %d CrC\r\n", my_nrf24l01p.getCrcWidth() );
00516     pc.printf( "nRF24L01+ TransferSize : %d Paket Size\r\n", my_nrf24l01p.getTransferSize () );
00517     my_nrf24l01p.setReceiveMode();
00518     my_nrf24l01p.enable();
00519     pc.printf( "Setup complete, Starting While loop\r\n");
00520 }
00521 
00522 
00523 /**
00524  * Receives a number from the Arduino.
00525  *
00526  * @return The value send by the arduino
00527  */
00528 double receiveValue(void)
00529 {
00530     char temp[4];
00531     double Val;
00532     bool ok=0;
00533     my_nrf24l01p.setTransferSize(4);
00534     my_nrf24l01p.setReceiveMode();
00535     my_nrf24l01p.enable();
00536     do {
00537         if(my_nrf24l01p.readable(NRF24L01P_PIPE_P0)) {
00538             ok = my_nrf24l01p.read( NRF24L01P_PIPE_P0,temp, 4);
00539         }
00540     } while(ok==0);
00541 
00542     //transformation of temp to convert to original value
00543     if(temp[0]==0) // if first elemente is 0 then its negative
00544         Val = double(-(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255));
00545     else // else its positive
00546         Val = double(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255);
00547 
00548     return Val;
00549 }
00550 
00551 
00552 /**
00553  * Sends a number to the Arduino
00554  *
00555  * @param Value - number to be sent to the Arduino
00556  */
00557 void sendValue(long int Value)
00558 {
00559     bool ok=0;  // comunication sucess, o if failed 1 if sucessfull
00560     // double math=Value/65025; // temporary variable save results
00561     int zero=1;  // 1 byte, ( - ) if 0 ( + ) if 1
00562     int one=0;   // 2 byte (0..255), multiplied by 1
00563     int two=0;   // 3 byte (0..255), multiplied by 255
00564     int three=0; // 4 byte (0..255), multiplied by 255*255
00565 
00566 //transformation of Value to send correctly through pipe
00567     if (Value<0) {
00568         zero=0;
00569         Value=abs(Value);
00570     }
00571     //  Value=abs(Value);
00572 
00573     double math=Value/65025; // temporary variable save results
00574 
00575     if(math<1) {
00576         math=Value/255;
00577         if(math<1) {
00578             two=0;
00579             one=Value;
00580         } else {
00581             two=(int)math;
00582             one=Value % 255;
00583         }
00584     } else {
00585         three=(int)math;
00586         math=Value/255;
00587         if(math<1) {
00588             two=0;
00589             one=Value;
00590         } else {
00591             two=(int)math;
00592             one=Value % 255;
00593         }
00594     }
00595     char temp[4]= {(int)zero,(int)one,(int)two,(int)three};
00596 
00597     // Apagar depois de testar mais vezes
00598     // pc.printf("1 inidice...%i...\r", temp[0]);
00599     // pc.printf("2 inidice...%i...\r", temp[1]);
00600     // pc.printf("3 inidice...%i...\r", temp[2]);
00601     // pc.printf("4 inidice...%i...\r", temp[3]);
00602 
00603     my_nrf24l01p.setTransferSize(4);
00604     my_nrf24l01p.setTransmitMode();
00605     my_nrf24l01p.enable();
00606     do {
00607         ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,temp, 4);
00608         if(ok==1)
00609             pc.printf("Done.....%i...\r", Value);
00610         else {
00611             pc.printf("Failed\r");
00612             wait(1);
00613         }
00614     } while(ok==0);
00615 }
00616 
00617 /**
00618  *  Sends matrix to arduino.
00619  *
00620  * @param matrix - Matrix of numbers to send [0..255]
00621  * @param row - Number of rows
00622  * @param column - Number of columns
00623  */
00624 void sendMatrix(int (*matrix)[18],int row , int column)
00625 {
00626     short ok=0;
00627     short int i =0;
00628     short int j=0;
00629     short int byte=0;
00630     int members=column*row;
00631     char message[32]= {0};
00632     pc.printf("J ...%d... \r",members);
00633 
00634     my_nrf24l01p.setTransferSize(32);
00635     my_nrf24l01p.setTransmitMode();
00636     my_nrf24l01p.enable();
00637 
00638     do {
00639         int* point = matrix[j];
00640 
00641         do {
00642             message[byte]= point[i];
00643             if (byte==31 || (i+1)*(j+1)==members) {
00644 
00645                 do {
00646                     ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,message, 32);
00647                     if(ok==0)
00648                         wait(1);
00649 
00650                 } while(ok==0);
00651 
00652                 byte=-1;
00653             }
00654 
00655             byte++;
00656             i++;
00657 
00658         } while(i<column);
00659 
00660         i=0;
00661         j++;
00662     } while(j<row);
00663 
00664 }
00665 
00666 ///////////////////////////////////////////////////////////////////////////////////////////////
00667 //////////////////////////////////      Sonar     ////////////////////////////////////////////
00668 ///////////////////////////////////////////////////////////////////////////////////////////////
00669 //      Commands of operation with ultrasonic module
00670 
00671 //    WRITE OPTION:
00672 //        ENABLE DC DC CONVERTER          - 0x0C;
00673 //        DISABLE DC DC CONVERTER         - 0x0B;
00674 //        START MEASURE LEFT SENSOR       - 0x0A;
00675 //        START MEASURE FRONT SENSOR      - 0x09;
00676 //        START MEASURE RIGHT SENSOR      - 0x08;
00677 //        SENSORS ALWAYS MEASURE ON       - 0x07;
00678 //        SENSORS ALWAYS MEASURE OFF      - 0x06;
00679 
00680 // READ OPTION:
00681 //        GET MEASURE OF LEFT SENSOR          - 0x05;
00682 //        GET MEASURE OF FRONT SENSOR         - 0x04;
00683 //        GET MEASURE OF IGHT SENSOR          - 0x03;
00684 //        GET STATUS SENSORS ALWAYS MEASURE   - 0x02;
00685 //        GET STATUS DC DC CONVERTER          - 0x01;
00686 
00687 void enable_dc_dc_boost()
00688 {
00689     char data[1];
00690     data[0]= 0x0C;
00691     wait_ms(1);
00692     i2c1.start();
00693     i2c1.write(0x30,data,1);
00694     i2c1.stop();
00695     i2c1.start();
00696     i2c1.write(0x30,data,1);
00697     i2c1.stop();
00698 }
00699 
00700 
00701 void disable_dc_dc_boost()
00702 {
00703     char data[1];
00704     data[0]= 0x0B;
00705     wait_ms(1);
00706     i2c1.start();
00707     i2c1.write(0x30,data,1);
00708     i2c1.stop();
00709 }
00710 
00711 
00712 void start_read_left_sensor()
00713 {
00714     char data[1];
00715     data[0]= 0x0A;
00716     wait_ms(1);
00717     i2c1.start();
00718     i2c1.write(0x30,data,1);
00719     i2c1.stop();
00720 }
00721 
00722 
00723 void start_read_front_sensor()
00724 {
00725     char data[1];
00726     data[0]= 0x09;
00727     wait_ms(1);
00728     i2c1.start();
00729     i2c1.write(0x30,data,1);
00730     i2c1.stop();
00731 }
00732 
00733 
00734 void start_read_right_sensor()
00735 {
00736     char data[1];
00737     data[0]= 0x08;
00738     wait_ms(1);
00739     i2c1.start();
00740     i2c1.write(0x30,data,1);
00741     i2c1.stop();
00742 }
00743 
00744 
00745 void measure_always_on()  // left, front, right
00746 {
00747     char data[1];
00748     data[0]= 0x07;
00749     wait_ms(1);
00750     i2c1.start();
00751     i2c1.write(0x30,data,1);
00752     i2c1.stop();
00753 }
00754 
00755 
00756 void measure_always_off()
00757 {
00758     char data[1];
00759     data[0]= 0x06;
00760     wait_ms(1);
00761     i2c1.start();
00762     i2c1.write(0x30,data,1);
00763     i2c1.stop();
00764 }
00765 
00766 /**
00767  * Returns left sensor value
00768  */
00769 static unsigned int get_distance_left_sensor()
00770 {
00771 
00772     static char data_r[3];
00773     static unsigned int aux;
00774     flag=1;
00775 
00776     data_r[0]= 0x05;
00777     wait_ms(1);
00778     i2c1.start();
00779     i2c1.write(0x30,data_r,1);
00780     i2c1.stop();
00781     wait_ms(10);
00782     i2c1.start();
00783     i2c1.read(0x31,data_r,2,0);
00784     i2c1.stop();
00785 
00786     aux=(data_r[0]*256)+data_r[1];
00787     flag=0;
00788     return aux;
00789     // sensor_left=aux;
00790     // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
00791 
00792 }
00793 
00794 
00795 /**
00796  * Returns front sensor value
00797  */
00798 static unsigned int get_distance_front_sensor()
00799 {
00800 
00801     static char data_r[3];
00802     static unsigned int aux;
00803     flag=1;
00804     data_r[0]= 0x04;
00805     wait_ms(1);
00806     i2c1.start();
00807     i2c1.write(0x30,data_r,1);
00808     i2c1.stop();
00809     wait_ms(10);
00810     i2c1.start();
00811     i2c1.read(0x31,data_r,2,0);
00812     i2c1.stop();
00813 
00814     aux=(data_r[0]*256)+data_r[1];
00815     flag=0;
00816     return aux;
00817     // sensor_front=aux;
00818     // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
00819 
00820 }
00821 
00822 
00823 /**
00824  * Returns right sensor value
00825  */
00826 static unsigned int get_distance_right_sensor()
00827 {
00828 
00829     static char data_r[3];
00830     static unsigned int aux;
00831     flag=1;
00832 
00833     data_r[0]= 0x03;
00834     wait_ms(1);
00835     i2c1.start();
00836     i2c1.write(0x30,data_r,1);
00837     i2c1.stop();
00838     wait_ms(10);
00839     i2c1.start();
00840     i2c1.read(0x31,data_r,2,0);
00841     i2c1.stop();
00842 
00843     aux=(data_r[0]*256)+data_r[1];
00844     flag=0;
00845     return aux;
00846     // sensor_right=aux;
00847     // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
00848 
00849 }
00850 
00851 
00852 void get_status_always_measure()
00853 {
00854 
00855     static char data_r[3];
00856     static unsigned int aux;
00857 
00858     data_r[0]= 0x02;
00859     wait_ms(1);
00860     i2c1.start();
00861     i2c1.write(0x30,data_r,1);
00862     i2c1.stop();
00863     wait_ms(10);
00864     i2c1.start();
00865     i2c1.read(0x31,data_r,2,0);
00866     i2c1.stop();
00867 
00868     aux=data_r[0];
00869     pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
00870 
00871 }
00872 
00873 
00874 void get_status_dcdc_converter()
00875 {
00876 
00877     static char data_r[3];
00878     static unsigned int aux;
00879 
00880     data_r[0]= 0x01;
00881     wait_ms(1);
00882     i2c1.start();
00883     i2c1.write(0x30,data_r,1);
00884     i2c1.stop();
00885     wait_ms(10);
00886     i2c1.start();
00887     i2c1.read(0x31,data_r,2,0);
00888     i2c1.stop();
00889 
00890     aux=data_r[0];
00891     pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
00892 
00893 }
00894 
00895 
00896 ///////////////////////////////////////////////////////////////////////////////////////////////
00897 //////////////////////////////////      MISC.      ////////////////////////////////////////////
00898 ///////////////////////////////////////////////////////////////////////////////////////////////
00899 
00900 
00901 /**
00902  * Initializes the necessary robot pins
00903  */
00904 void init_robot_pins()
00905 {
00906 
00907     //SAIDAS DIGITAIS (normal)
00908     //q_pha_mot_rig=0;            //Phase Motor Right
00909     //q_sleep_mot_rig=0;          //Nano Sleep Motor Right
00910     //q_pha_mot_lef=0;            //Phase Motor Left
00911     //q_sleep_mot_lef=0;          //Nano Sleep Motor Left
00912     //q_pow_ena_i2c_p=0;          //Power Enable i2c FET P
00913     //q_pow_ena_mic_p=0;          //Power enable Micro FET P
00914     //q_pow_as5600_n=1;           //AS5600 Power MOSFET N
00915     //q_pow_as5600_p=0;           //AS5600 Power MOSFET P
00916     //q_pow_spi=0;                //SPI Power MOSFET P
00917     //q_ena_mppt=0;               //Enable MPPT Control
00918     //q_boost_ps=1;               //Boost Power Save
00919     //q_tca9548_reset=1;          //Reset TCA9548
00920 
00921     //SAIDAS DIGITAIS (normal)
00922     q_pha_mot_rig=0;            //Phase Motor Right
00923     q_sleep_mot_rig=0;          //Nano Sleep Motor Right
00924     q_pha_mot_lef=0;            //Phase Motor Left
00925     q_sleep_mot_lef=0;          //Nano Sleep Motor Left
00926 
00927     q_pow_ena_i2c_p=0;         //Power Enable i2c FET P
00928     q_pow_ena_mic_p=0;          //Power enable Micro FET P
00929     q_pow_as5600_p=0;           //AS5600 Power MOSFET P
00930     // q_pow_spi=0;                //SPI Power MOSFET P
00931     q_pow_as5600_n=1;           //AS5600 Power MOSFET N
00932 
00933 
00934     q_ena_mppt=0;               //Enable MPPT Control
00935     q_boost_ps=1;               //Boost Power Save
00936     q_tca9548_reset=1;          //Reset TCA9548
00937 
00938     //Leds caso seja saida digital:
00939     q_led_red_fro=1;          //Led Red Front (led off)
00940     q_led_gre_fro=1;          //Led Green Front (led off)
00941     q_led_blu_fro=1;          //Led Blue Front (led off)
00942     q_led_red_rea=1;          //Led Red Rear (led off)
00943     q_led_gre_rea=1;          //Led Green Rear (led off)
00944     q_led_blu_rea=1;          //Led Blue Rear (led off)r
00945 
00946 
00947 //********************************************************************
00948     //SAIDAS DIGITAIS (pwm)
00949     //PWM Enable Motor Right
00950     pwm_mot_rig.period_us(500);
00951     pwm_mot_rig.pulsewidth_us(0);
00952 
00953     //PWM Enable Motor Left
00954     pwm_mot_lef.period_us(500);
00955     pwm_mot_lef.pulsewidth_us(0);
00956 
00957     //Buzzer PWM
00958     pwm_buzzer.period_us(500);
00959     pwm_buzzer.pulsewidth_us(0);
00960 
00961     //LED white
00962     pwm_led_whi.period_us(500);
00963     pwm_led_whi.pulsewidth_us(0);
00964 
00965 }
00966 
00967 
00968 /**
00969  * Initializes all the pins and all the modules necessary
00970  */
00971 void initRobot(void)
00972 {
00973     init_robot_pins();
00974     enable_dc_dc_boost();
00975     init_Infrared();
00976     initEncoders();
00977     config_init_nrf();
00978     enable_dc_dc_boost();
00979     wait_ms(100); //wait for read wait(>=150ms);
00980     measure_always_on();
00981     float value = value_of_Batery();
00982     pc.printf("Initialization Successful \n\r");
00983     pc.printf("Battery level: %f \n\r",value);
00984     if(value < 3.0) {
00985         pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
00986     }
00987 
00988     // float level = value_of_Batery();
00989     // sendValue(int(level*100));
00990 
00991 }
00992 
00993 
00994 ////////////////////////////////////////////////////
00995 
00996 /**
00997  * Updates the position and orientation of the robot based on the data from the encoders
00998  *
00999  * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
01000  * and the distance between them is 7.4
01001  */
01002 void Odometria()
01003 {
01004     long int ticks1d=R_encoder();
01005     long int ticks1e=L_encoder();
01006 
01007     long int D_ticks=ticks1d - ticks2d;
01008     long int E_ticks=ticks1e - ticks2e;
01009 
01010     ticks2d=ticks1d;
01011     ticks2e=ticks1e;
01012 
01013     float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
01014     float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
01015 
01016     float CM=(D_cm + L_cm)/2;
01017 
01018     theta +=(D_cm - L_cm)/7.18;
01019 
01020     theta = atan2(sin(theta), cos(theta));
01021 
01022     // meter entre 0
01023 
01024     X += CM*cos(theta);
01025     Y += CM*sin(theta);
01026 
01027 }