Fabio Faria
/
ISR_Mini-explorer_tmp
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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 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; 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 thread.start(odometry_thread); 00563 00564 float value = value_of_Batery(); 00565 00566 pc.printf("Initialization Successful \n\r"); 00567 pc.printf("Battery level: %f \n\r",value); 00568 if(value < 3.0) { 00569 pc.printf(" WARNING: BATTERY NEEDS CHARGING "); 00570 } 00571 // float level = value_of_Batery(); 00572 // sendValue(int(level*100)); 00573 00574 }
Generated on Thu Jul 28 2022 17:26:36 by
1.7.2