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