Luis Osorio
Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Fork of
ISR_Mini-explorer
by 
ISR UC
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 20:38:56 by
1.7.2