robot.h

00001 /** @file */
00002 
00003 #include "mbed.h"
00004 #include <math.h>
00005 #include "VCNL40x0.h"
00006 #include "Radio.h"
00007 #include "Encoder.h"
00008 #include "Rangefinder.h"
00009 
00010 I2C i2c(PTC9,PTC8);
00011 I2C i2c1(PTC11,PTC10);
00012 Mutex mutex_i2c0;
00013 Mutex mutex_i2c1;
00014 
00015 float X=20;
00016 float Y=20;
00017 float THETA=0;
00018 Encoder esquerdo(&i2c, &mutex_i2c0, 0);
00019 Encoder direito(&i2c1, &mutex_i2c1, 1);
00020 Thread thread;
00021 
00022 void odometry_thread()
00023 {
00024     float theta=0;
00025     long int ticks2d=0;
00026     long int ticks2e=0;
00027 
00028 
00029 
00030     while (true) {
00031         esquerdo.incremental();
00032         direito.incremental();
00033         //-------------------------------------------
00034         long int ticks1d=direito.readIncrementalValue();
00035         long int ticks1e=esquerdo.readIncrementalValue();
00036 
00037         long int D_ticks=ticks1d - ticks2d;
00038         long int E_ticks=ticks1e - ticks2e;
00039 
00040         ticks2d=ticks1d;
00041         ticks2e=ticks1e;
00042 
00043         float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
00044         float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
00045 
00046         float CM=(D_cm + L_cm)/2;
00047 
00048         theta +=(D_cm - L_cm)/7.18;
00049 
00050         theta = atan2(sin(theta), cos(theta));
00051         THETA = theta;
00052 
00053         // meter entre 0
00054 
00055         X += CM*cos(theta);
00056         Y += CM*sin(theta);
00057         //-------------------------------------------
00058 
00059         Thread::wait(10);
00060     }
00061 }
00062 
00063 // classes adicionais
00064 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
00065 Serial pc(PTE0,PTE1);
00066 
00067 
00068 // Variables needed by the lib
00069 unsigned int  ProxiValue=0;
00070 
00071 //SAIDAS DIGITAIS (normal)
00072 DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
00073 DigitalOut  q_sleep_mot_rig     (PTE3,0);     //Nano Sleep Motor Right
00074 DigitalOut  q_pha_mot_lef       (PTA17,0);    //Phase Motor Left
00075 DigitalOut  q_sleep_mot_lef     (PTB11,0);    //Nano Sleep Motor Left
00076 DigitalOut  q_pow_ena_i2c_p     (PTE2,0);     //Power Enable i2c FET P (0- enable 1-disable)
00077 DigitalOut  q_pow_ena_mic_p     (PTA14,0);    //Power enable Micro FET P (0- enable 1-disable)
00078 DigitalOut  q_pow_as5600_n      (PTC6,1);     //AS5600 Power MOSFET N (1- enable 0-disable)
00079 DigitalOut  q_pow_as5600_p      (PTC5,0);     //AS5600 Power MOSFET P (0- enable 1-disable)
00080 DigitalOut  q_pow_spi           (PTC4,0);     //SPI Power MOSFET P (0- enable 1-disable)
00081 DigitalOut  q_ena_mppt          (PTC0,0);     //Enable MPPT Control (0- enable 1-disable)
00082 DigitalOut  q_boost_ps          (PTC7,1);     //Boost Power Save (1- enable 0-disable)
00083 DigitalOut  q_tca9548_reset     (PTC3,1);     //Reset TCA9548 (1- enable 0-disable)
00084 DigitalOut  power_36khz         (PTD0,0);     //Power enable pic12f - 36khz (0- enable 1-disable)
00085 
00086 
00087 // ********************************************************************
00088 // ********************************************************************
00089 //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
00090 //ENTRADAS DIGITAIS (normal input)
00091 DigitalIn   i_enc_dir_rig       (PTB8);     //Encoder Right Direction
00092 DigitalIn   i_enc_dir_lef       (PTB9);     //Encoder Left Direction
00093 DigitalIn   i_micro_sd_det      (PTC16);    //MICRO SD Card Detect
00094 DigitalIn   i_mppt_fail         (PTE5);     //Fail MPPT Signal
00095 DigitalIn   i_usb_volt          (PTB10);    //USB Voltage detect
00096 DigitalIn   i_sup_cap_est       (PTB19);    //Supercap State Charger
00097 DigitalIn   i_li_ion_est        (PTB18);    //Li-ion State Charger
00098 
00099 
00100 // ********************************************************************
00101 //ENTRADAS DIGITAIS (external interrupt)
00102 InterruptIn i_int_mpu9250       (PTA15);    //Interrupt MPU9250
00103 InterruptIn i_int_isl29125      (PTA16);    //Interrupt ISL29125 Color S.
00104 InterruptIn i_mic_f_l           (PTD7);     //Interrupt Comp Micro F L
00105 InterruptIn i_mic_f_r           (PTD6);     //Interrupt Comp Micro F R
00106 InterruptIn i_mic_r_c           (PTD5);     //Interrupt Comp Micro R C
00107 
00108 
00109 // ********************************************************************
00110 //ENTRADAS ANALOGICAS
00111 AnalogIn    a_enc_rig           (PTC2);     //Encoder Left Output_AS_MR
00112 AnalogIn    a_enc_lef           (PTC1);     //Encoder Right Output_AS_ML
00113 AnalogIn    a_mic_f_l           (PTB0);     //Analog microphone F L
00114 AnalogIn    a_mic_f_r           (PTB1);     //Analog microphone F R
00115 AnalogIn    a_mic_r_c           (PTB2);     //Analog microphone R C
00116 AnalogIn    a_temp_bat          (PTB3);     //Temperature Battery
00117 
00118 
00119 // ********************************************************************
00120 
00121 //PWM OR DIGITAL OUTPUT NORMAL
00122 //DigitalOut    q_led_whi         (PTE29);    //Led white pwm
00123 DigitalOut    q_led_red_fro     (PTA4);     //Led Red Front
00124 DigitalOut    q_led_gre_fro     (PTA5);     //Led Green Front
00125 DigitalOut    q_led_blu_fro     (PTA12);    //Led Blue Front
00126 DigitalOut    q_led_red_rea     (PTD4);     //Led Red Rear
00127 DigitalOut    q_led_gre_rea     (PTA1);     //Led Green Rear
00128 DigitalOut    q_led_blu_rea     (PTA2);     //Led Blue Rear
00129 
00130 //SAIDAS DIGITAIS (pwm)
00131 PwmOut      pwm_mot_rig         (PTE20);    //PWM Enable Motor Right
00132 PwmOut      pwm_mot_lef         (PTE31);    //PWM Enable Motor Left
00133 PwmOut      pwm_buzzer          (PTE21);    //Buzzer PWM
00134 PwmOut      pwm_led_whi         (PTE29);    //Led white pwm
00135 
00136 // ********************************************************************
00137 //SAIDAS ANALOGICAS
00138 AnalogOut   dac_comp_mic        (PTE30);        //Dac_Comparator MIC
00139 
00140 
00141 
00142 
00143 /**
00144  * Selects the wich infrared to comunicate.
00145  *
00146  * @param ch - Infrared to read (1..5)
00147  */
00148 void tca9548_select_ch(char ch)
00149 {
00150     char ch_f[1];
00151     char addr=0xE0;
00152 
00153     if(ch==0)
00154         ch_f[0]=1;
00155 
00156     if(ch>=1)
00157         ch_f[0]=1<<ch;
00158 
00159 mutex_i2c0.lock();
00160     i2c.write(addr,ch_f,1);
00161     mutex_i2c0.unlock();
00162 }
00163 
00164 
00165 
00166 
00167 
00168 /* Powers up all the VCNL4020. */
00169 void init_Infrared()
00170 {
00171     
00172     for (int i=0; i<6;i++)
00173     {
00174             tca9548_select_ch(i);
00175             VCNL40x0_Device.SetCurrent (20);     // Set current to 200mA
00176     }
00177     tca9548_select_ch(0);
00178 }
00179 
00180 
00181 /**
00182  * Get the distance from of the chosen infrared.
00183  *
00184  * @param ch - Infrared to read (0,1..5)
00185  *
00186  * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
00187  */
00188 float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
00189 {
00190     tca9548_select_ch(ch);
00191     VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue);    // read prox value on demand
00192     float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
00193     return aux;
00194     //return ProxiValue;
00195 }
00196 
00197 ///////////////////////////////////////////////////////////////////////////////////////////////
00198 ///////////////////////////////////     MOTOR       ///////////////////////////////////////////
00199 ///////////////////////////////////////////////////////////////////////////////////////////////
00200 
00201 // Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
00202 // consultar pag 39 e 95
00203 
00204 /**
00205  * Sets speed and direction of the left motor.
00206  *
00207  * @param Dir - Direction of movement, 0 for back, or 1 for fron
00208  * @param Speed - Percentage of speed of the motor (1..100)
00209  *
00210  * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
00211  *  at different speeds and see if it makes a straigth line
00212  */
00213 void leftMotor(short int Dir,short int Speed)
00214 {
00215     float Duty;
00216 
00217     if(Dir==1) {
00218         q_pha_mot_lef=0;            //Andar em frente
00219         if(Speed>1000)                   //limite de segurança
00220             Speed=1000;
00221         if(Speed>0) {
00222             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00223             q_sleep_mot_lef=1;          //Nano Sleep Motor Left
00224             pwm_mot_lef.pulsewidth_us(Duty*5);
00225         } else {
00226             q_sleep_mot_lef=0;
00227         }
00228     }
00229     if(Dir==0) {
00230         q_pha_mot_lef=1;            //Andar para tras
00231 
00232         if(Speed>1000)                   //limite de segurança
00233             Speed=1000;
00234         if(Speed>0) {
00235             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00236             q_sleep_mot_lef=1;          //Nano Sleep Motor Left
00237             pwm_mot_lef.pulsewidth_us(Duty*5);
00238         } else {
00239             q_sleep_mot_lef=0;
00240         }
00241     }
00242 }
00243 
00244 
00245 /**
00246  * Sets speed and direction of the right motor.
00247  *
00248  * @param Dir - Direction of movement, 0 for back, or 1 for fron
00249  * @param Speed - Percentage of speed of the motor (1..100)
00250  *
00251  * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
00252  *  at different speeds and see if it makes a straigth line
00253  */
00254 void rightMotor(short int Dir,short int Speed)
00255 {
00256     float Duty;
00257 
00258     if(Dir==1) {
00259         q_pha_mot_rig=0;            //Andar em frente
00260 
00261         if(Speed>1000)                   //limite de segurança
00262             Speed=1000;
00263         if(Speed>0) {
00264             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00265             q_sleep_mot_rig=1;          //Nano Sleep Motor Right
00266             pwm_mot_rig.pulsewidth_us(Duty*5);
00267         } else {
00268             q_sleep_mot_rig=0;
00269         }
00270     }
00271     if(Dir==0) {
00272         q_pha_mot_rig=1;            //Andar para tras
00273 
00274 
00275         if(Speed>1000)                   //limite de segurança
00276             Speed=1000;
00277         if(Speed>0) {
00278             Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
00279             q_sleep_mot_rig=1;          //Nano Sleep Motor Right
00280             pwm_mot_rig.pulsewidth_us(Duty*5);
00281         } else {
00282             q_sleep_mot_rig=0;
00283         }
00284     }
00285 }
00286 
00287 
00288 ///////////////////////////////////////////////////////////////////////////////////////////////
00289 ///////////////////////////////////     BATTERY     ///////////////////////////////////////////
00290 ///////////////////////////////////////////////////////////////////////////////////////////////
00291 
00292 /**
00293  * Reads adc of the battery.
00294  *
00295  * @param addr - Address to read
00296  * @return The voltage of the baterry
00297  */
00298 long int read16_mcp3424(char addr)
00299 {
00300     char data[4];
00301     mutex_i2c1.lock();
00302     i2c1.read(addr,data,3);
00303     mutex_i2c1.unlock();
00304     return(((data[0]&127)*256)+data[1]);
00305 }
00306 
00307 /**
00308  * Reads adc of the battery.
00309  *
00310  * @param n_bits - Resolution of measure
00311  * @param ch - Chose value to read, if voltage or current of solar or batery
00312  * @param gain -
00313  * @param addr - Address to write to
00314  */
00315 void write_mcp3424(int n_bits, int  ch, int gain, char  addr)  //chanel 1-4    write -> 0xD0
00316 {
00317 
00318     int chanel_end=(ch-1)<<5; //shift left
00319     char n_bits_end=0;
00320 
00321     if(n_bits==12) {
00322         n_bits_end=0;
00323     } else if(n_bits==14) {
00324         n_bits_end=1;
00325     } else if(n_bits==16) {
00326         n_bits_end=2;
00327     } else {
00328         n_bits_end=3;
00329     }
00330     n_bits_end=n_bits_end<<2; //shift left
00331 
00332     char data[1];
00333     data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
00334     mutex_i2c1.lock();
00335     i2c1.write(addr,data,1);
00336     mutex_i2c1.unlock();
00337 }
00338 
00339 
00340 /**
00341  * Reads adc of the battery.
00342  *
00343  * @return The voltage of the batery
00344  */
00345 float value_of_Batery()
00346 {
00347     float   R1=75000.0;
00348     float   R2=39200.0;
00349     float   R3=178000.0;
00350     float   Gain=1.0;
00351     write_mcp3424(16,3,1,0xd8);
00352     float cha3_v2=read16_mcp3424(0xd9); //read  voltage
00353     float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
00354     float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
00355     Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
00356 
00357     return Vin_b_v_battery;
00358 }
00359 
00360 ///////////////////////////////////////////////////////////////////////////////////////////////
00361 //////////////////////////////////      Sonar     ////////////////////////////////////////////
00362 ///////////////////////////////////////////////////////////////////////////////////////////////
00363 //      Commands of operation with ultrasonic module
00364 
00365 //    WRITE OPTION:
00366 //        ENABLE DC DC CONVERTER          - 0x0C;
00367 //        DISABLE DC DC CONVERTER         - 0x0B;
00368 //        START MEASURE LEFT SENSOR       - 0x0A;
00369 //        START MEASURE FRONT SENSOR      - 0x09;
00370 //        START MEASURE RIGHT SENSOR      - 0x08;
00371 //        SENSORS ALWAYS MEASURE ON       - 0x07;
00372 //        SENSORS ALWAYS MEASURE OFF      - 0x06;
00373 
00374 // READ OPTION:
00375 //        GET MEASURE OF LEFT SENSOR          - 0x05;
00376 //        GET MEASURE OF FRONT SENSOR         - 0x04;
00377 //        GET MEASURE OF IGHT SENSOR          - 0x03;
00378 //        GET STATUS SENSORS ALWAYS MEASURE   - 0x02;
00379 //        GET STATUS DC DC CONVERTER          - 0x01;
00380 
00381 void enable_dc_dc_boost()
00382 {
00383     char data[1];
00384     data[0]= 0x0C;
00385     mutex_i2c1.lock();
00386     i2c1.write(0x30,data,1);
00387     mutex_i2c1.unlock();
00388 }
00389 
00390 
00391 void measure_always_on()  // left, front, right
00392 {
00393     char data[1];
00394     data[0]= 0x07;
00395     mutex_i2c1.lock();
00396     i2c1.write(0x30,data,1);
00397     mutex_i2c1.unlock();
00398 }
00399 
00400 
00401 /**
00402  * Returns left sensor value
00403  */
00404 static unsigned int get_distance_left_sensor()
00405 {
00406       
00407     static char data_r[3];
00408     static unsigned int aux;
00409     
00410     data_r[0]= 0x05;
00411     mutex_i2c1.lock();
00412     i2c1.write(0x30,data_r,1);
00413     i2c1.read(0x31,data_r,2);
00414     mutex_i2c1.unlock();
00415 
00416     aux=(data_r[0]*256)+data_r[1];
00417 
00418     return aux;
00419 }
00420 
00421 
00422 /**
00423  * Returns front sensor value
00424  */
00425 static unsigned int get_distance_front_sensor()
00426 {
00427 
00428     static char data_r[3];
00429     static unsigned int aux;
00430 
00431     data_r[0]= 0x04;
00432     
00433     mutex_i2c1.lock();
00434     i2c1.write(0x30,data_r,1);
00435     i2c1.read(0x31,data_r,2);
00436     mutex_i2c1.unlock();
00437 
00438     aux=(data_r[0]*256)+data_r[1];
00439 
00440     return aux;
00441 
00442 }
00443 
00444 
00445 /**
00446  * Returns right sensor value
00447  */
00448 static unsigned int get_distance_right_sensor()
00449 {
00450 
00451     static char data_r[3];
00452     static unsigned int aux;
00453 
00454     data_r[0]= 0x03;
00455 
00456     mutex_i2c1.lock();
00457     i2c1.write(0x30,data_r,1);
00458     i2c1.read(0x31,data_r,2,0);
00459     mutex_i2c1.unlock();
00460 
00461     aux=(data_r[0]*256)+data_r[1];
00462 
00463     return aux;
00464 
00465 }
00466 
00467 
00468 
00469 ///////////////////////////////////////////////////////////////////////////////////////////////
00470 //////////////////////////////////      MISC.      ////////////////////////////////////////////
00471 ///////////////////////////////////////////////////////////////////////////////////////////////
00472 
00473 
00474 /**
00475  * Initializes the necessary robot pins
00476  */
00477 void init_robot_pins()
00478 {
00479 
00480     //SAIDAS DIGITAIS (normal)
00481     //q_pha_mot_rig=0;            //Phase Motor Right
00482     //q_sleep_mot_rig=0;          //Nano Sleep Motor Right
00483     //q_pha_mot_lef=0;            //Phase Motor Left
00484     //q_sleep_mot_lef=0;          //Nano Sleep Motor Left
00485     //q_pow_ena_i2c_p=0;          //Power Enable i2c FET P
00486     //q_pow_ena_mic_p=0;          //Power enable Micro FET P
00487     //q_pow_as5600_n=1;           //AS5600 Power MOSFET N
00488     //q_pow_as5600_p=0;           //AS5600 Power MOSFET P
00489     //q_pow_spi=0;                //SPI Power MOSFET P
00490     //q_ena_mppt=0;               //Enable MPPT Control
00491     //q_boost_ps=1;               //Boost Power Save
00492     //q_tca9548_reset=1;          //Reset TCA9548
00493 
00494     //SAIDAS DIGITAIS (normal)
00495     q_pha_mot_rig=0;            //Phase Motor Right
00496     q_sleep_mot_rig=0;          //Nano Sleep Motor Right
00497     q_pha_mot_lef=0;            //Phase Motor Left
00498     q_sleep_mot_lef=0;          //Nano Sleep Motor Left
00499 
00500     q_pow_ena_i2c_p=0;         //Power Enable i2c FET P
00501     q_pow_ena_mic_p=0;          //Power enable Micro FET P
00502     q_pow_as5600_p=0;           //AS5600 Power MOSFET P
00503     // q_pow_spi=0;                //SPI Power MOSFET P
00504     q_pow_as5600_n=1;           //AS5600 Power MOSFET N
00505 
00506 
00507     q_ena_mppt=0;               //Enable MPPT Control
00508     q_boost_ps=1;               //Boost Power Save
00509     q_tca9548_reset=1;          //Reset TCA9548
00510 
00511     //Leds caso seja saida digital:
00512     q_led_red_fro=1;          //Led Red Front (led off)
00513     q_led_gre_fro=1;          //Led Green Front (led off)
00514     q_led_blu_fro=1;          //Led Blue Front (led off)
00515     q_led_red_rea=1;          //Led Red Rear (led off)
00516     q_led_gre_rea=1;          //Led Green Rear (led off)
00517     q_led_blu_rea=1;          //Led Blue Rear (led off)r
00518 
00519 
00520 //********************************************************************
00521     //SAIDAS DIGITAIS (pwm)
00522     //PWM Enable Motor Right
00523     pwm_mot_rig.period_us(500);
00524     pwm_mot_rig.pulsewidth_us(0);
00525 
00526     //PWM Enable Motor Left
00527     pwm_mot_lef.period_us(500);
00528     pwm_mot_lef.pulsewidth_us(0);
00529 
00530     //Buzzer PWM
00531     pwm_buzzer.period_us(500);
00532     pwm_buzzer.pulsewidth_us(0);
00533 
00534 
00535     //LED white
00536     pwm_led_whi.period_us(500);
00537     pwm_led_whi.pulsewidth_us(0);
00538 
00539 }
00540 
00541 /**
00542  * Initializes all the pins and all the modules necessary
00543  *
00544  * @param channel - Which RF channel to comunicate on, 0-125.
00545  *\note If you are not using RF module put random number.
00546  * \warning Channel on Robot and Board has to be the same.
00547  */
00548 void initRobot(int channel)
00549 {   
00550     pc.printf("Battery level: \n\r");
00551     init_nRF(channel);
00552     init_robot_pins();
00553     
00554     enable_dc_dc_boost();
00555     wait_ms(100); //wait for read wait(>=150ms);
00556     enable_dc_dc_boost();
00557     measure_always_on();
00558     wait_ms(100); //wait for read wait(>=150ms);
00559    
00560     init_Infrared();
00561     
00562     init_rangefinder(&i2c1, &mutex_i2c1);
00563     
00564     
00565     thread.start(odometry_thread);
00566     
00567     float value = value_of_Batery();
00568     
00569     pc.printf("Initialization Successful \n\r");
00570     pc.printf("Battery level: %f \n\r",value);
00571     if(value < 3.0) {
00572         pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
00573     }
00574     // float level = value_of_Batery();
00575     // sendValue(int(level*100));
00576 
00577 }