robot.h

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