Fabio Faria / ISR_Mini-explorer_tmp2

Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.

Dependencies:   RF24

Dependents:   ISR_Mini-explorer_Rangefinder

Fork of ISR_Mini-explorer by ISR UC

robot.h@7:95bee84b6910, 2018-05-16 (annotated)

Committer:
fabiofaria
Date:
Wed May 16 14:23:26 2018 +0000
Revision:
7:95bee84b6910
Parent:
5:43215ff15665 
Bug fixed on read_radio.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ISR 0:15a30802e719 1 /** @file */
ISR 0:15a30802e719 2
ISR 3:215c9544480d 3 #include "mbed.h"
ISR 0:15a30802e719 4 #include <math.h>
ISR 0:15a30802e719 5 #include "VCNL40x0.h"
fabiofaria 4:f6fddeae358e 6 #include "Radio.h"
ISR 1:8569ac717e68 7 #include "Encoder.h"
fabiofaria 4:f6fddeae358e 8 #include "Rangefinder.h"
ISR 1:8569ac717e68 9
ISR 1:8569ac717e68 10 I2C i2c(PTC9,PTC8);
ISR 1:8569ac717e68 11 I2C i2c1(PTC11,PTC10);
fabiofaria 4:f6fddeae358e 12 Mutex mutex_i2c0;
fabiofaria 4:f6fddeae358e 13 Mutex mutex_i2c1;
ISR 1:8569ac717e68 14
fabiofaria 4:f6fddeae358e 15 float X=20;
fabiofaria 4:f6fddeae358e 16 float Y=20;
fabiofaria 5:43215ff15665 17 float THETA=0;
fabiofaria 4:f6fddeae358e 18 Encoder esquerdo(&i2c, &mutex_i2c0, 0);
fabiofaria 4:f6fddeae358e 19 Encoder direito(&i2c1, &mutex_i2c1, 1);
fabiofaria 4:f6fddeae358e 20 Thread thread;
ISR 1:8569ac717e68 21
ISR 1:8569ac717e68 22 void odometry_thread()
ISR 1:8569ac717e68 23 {
ISR 1:8569ac717e68 24 float theta=0;
ISR 1:8569ac717e68 25 long int ticks2d=0;
ISR 1:8569ac717e68 26 long int ticks2e=0;
ISR 1:8569ac717e68 27
ISR 1:8569ac717e68 28
ISR 0:15a30802e719 29
ISR 1:8569ac717e68 30 while (true) {
ISR 1:8569ac717e68 31 esquerdo.incremental();
ISR 1:8569ac717e68 32 direito.incremental();
ISR 1:8569ac717e68 33 //-------------------------------------------
ISR 1:8569ac717e68 34 long int ticks1d=direito.readIncrementalValue();
ISR 1:8569ac717e68 35 long int ticks1e=esquerdo.readIncrementalValue();
ISR 1:8569ac717e68 36
ISR 1:8569ac717e68 37 long int D_ticks=ticks1d - ticks2d;
ISR 1:8569ac717e68 38 long int E_ticks=ticks1e - ticks2e;
ISR 1:8569ac717e68 39
ISR 1:8569ac717e68 40 ticks2d=ticks1d;
ISR 1:8569ac717e68 41 ticks2e=ticks1e;
ISR 1:8569ac717e68 42
ISR 1:8569ac717e68 43 float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
ISR 1:8569ac717e68 44 float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
ISR 1:8569ac717e68 45
ISR 1:8569ac717e68 46 float CM=(D_cm + L_cm)/2;
ISR 1:8569ac717e68 47
ISR 1:8569ac717e68 48 theta +=(D_cm - L_cm)/7.18;
ISR 1:8569ac717e68 49
ISR 1:8569ac717e68 50 theta = atan2(sin(theta), cos(theta));
ISR 2:0435d1171673 51 THETA = theta;
ISR 1:8569ac717e68 52
ISR 1:8569ac717e68 53 // meter entre 0
ISR 1:8569ac717e68 54
ISR 1:8569ac717e68 55 X += CM*cos(theta);
ISR 1:8569ac717e68 56 Y += CM*sin(theta);
ISR 1:8569ac717e68 57 //-------------------------------------------
ISR 1:8569ac717e68 58
ISR 1:8569ac717e68 59 Thread::wait(10);
ISR 1:8569ac717e68 60 }
ISR 1:8569ac717e68 61 }
ISR 1:8569ac717e68 62
ISR 0:15a30802e719 63 // classes adicionais
ISR 0:15a30802e719 64 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
ISR 0:15a30802e719 65 Serial pc(PTE0,PTE1);
ISR 1:8569ac717e68 66
ISR 0:15a30802e719 67
ISR 0:15a30802e719 68 // Variables needed by the lib
ISR 0:15a30802e719 69 unsigned int ProxiValue=0;
ISR 0:15a30802e719 70
ISR 0:15a30802e719 71 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 72 DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right
ISR 0:15a30802e719 73 DigitalOut q_sleep_mot_rig (PTE3,0); //Nano Sleep Motor Right
ISR 0:15a30802e719 74 DigitalOut q_pha_mot_lef (PTA17,0); //Phase Motor Left
ISR 0:15a30802e719 75 DigitalOut q_sleep_mot_lef (PTB11,0); //Nano Sleep Motor Left
ISR 0:15a30802e719 76 DigitalOut q_pow_ena_i2c_p (PTE2,0); //Power Enable i2c FET P (0- enable 1-disable)
ISR 0:15a30802e719 77 DigitalOut q_pow_ena_mic_p (PTA14,0); //Power enable Micro FET P (0- enable 1-disable)
ISR 0:15a30802e719 78 DigitalOut q_pow_as5600_n (PTC6,1); //AS5600 Power MOSFET N (1- enable 0-disable)
ISR 0:15a30802e719 79 DigitalOut q_pow_as5600_p (PTC5,0); //AS5600 Power MOSFET P (0- enable 1-disable)
ISR 0:15a30802e719 80 DigitalOut q_pow_spi (PTC4,0); //SPI Power MOSFET P (0- enable 1-disable)
ISR 0:15a30802e719 81 DigitalOut q_ena_mppt (PTC0,0); //Enable MPPT Control (0- enable 1-disable)
ISR 0:15a30802e719 82 DigitalOut q_boost_ps (PTC7,1); //Boost Power Save (1- enable 0-disable)
ISR 0:15a30802e719 83 DigitalOut q_tca9548_reset (PTC3,1); //Reset TCA9548 (1- enable 0-disable)
ISR 0:15a30802e719 84 DigitalOut power_36khz (PTD0,0); //Power enable pic12f - 36khz (0- enable 1-disable)
ISR 0:15a30802e719 85
ISR 0:15a30802e719 86
ISR 0:15a30802e719 87 // ********************************************************************
ISR 0:15a30802e719 88 // ********************************************************************
ISR 0:15a30802e719 89 //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
ISR 0:15a30802e719 90 //ENTRADAS DIGITAIS (normal input)
ISR 0:15a30802e719 91 DigitalIn i_enc_dir_rig (PTB8); //Encoder Right Direction
ISR 0:15a30802e719 92 DigitalIn i_enc_dir_lef (PTB9); //Encoder Left Direction
ISR 0:15a30802e719 93 DigitalIn i_micro_sd_det (PTC16); //MICRO SD Card Detect
ISR 0:15a30802e719 94 DigitalIn i_mppt_fail (PTE5); //Fail MPPT Signal
ISR 0:15a30802e719 95 DigitalIn i_usb_volt (PTB10); //USB Voltage detect
ISR 0:15a30802e719 96 DigitalIn i_sup_cap_est (PTB19); //Supercap State Charger
ISR 0:15a30802e719 97 DigitalIn i_li_ion_est (PTB18); //Li-ion State Charger
ISR 0:15a30802e719 98
ISR 0:15a30802e719 99
ISR 0:15a30802e719 100 // ********************************************************************
ISR 0:15a30802e719 101 //ENTRADAS DIGITAIS (external interrupt)
ISR 0:15a30802e719 102 InterruptIn i_int_mpu9250 (PTA15); //Interrupt MPU9250
ISR 0:15a30802e719 103 InterruptIn i_int_isl29125 (PTA16); //Interrupt ISL29125 Color S.
ISR 0:15a30802e719 104 InterruptIn i_mic_f_l (PTD7); //Interrupt Comp Micro F L
ISR 0:15a30802e719 105 InterruptIn i_mic_f_r (PTD6); //Interrupt Comp Micro F R
ISR 0:15a30802e719 106 InterruptIn i_mic_r_c (PTD5); //Interrupt Comp Micro R C
ISR 0:15a30802e719 107
ISR 0:15a30802e719 108
ISR 0:15a30802e719 109 // ********************************************************************
ISR 0:15a30802e719 110 //ENTRADAS ANALOGICAS
ISR 0:15a30802e719 111 AnalogIn a_enc_rig (PTC2); //Encoder Left Output_AS_MR
ISR 0:15a30802e719 112 AnalogIn a_enc_lef (PTC1); //Encoder Right Output_AS_ML
ISR 0:15a30802e719 113 AnalogIn a_mic_f_l (PTB0); //Analog microphone F L
ISR 0:15a30802e719 114 AnalogIn a_mic_f_r (PTB1); //Analog microphone F R
ISR 0:15a30802e719 115 AnalogIn a_mic_r_c (PTB2); //Analog microphone R C
ISR 0:15a30802e719 116 AnalogIn a_temp_bat (PTB3); //Temperature Battery
ISR 0:15a30802e719 117
ISR 0:15a30802e719 118
ISR 0:15a30802e719 119 // ********************************************************************
ISR 0:15a30802e719 120
ISR 0:15a30802e719 121 //PWM OR DIGITAL OUTPUT NORMAL
ISR 0:15a30802e719 122 //DigitalOut q_led_whi (PTE29); //Led white pwm
ISR 0:15a30802e719 123 DigitalOut q_led_red_fro (PTA4); //Led Red Front
ISR 0:15a30802e719 124 DigitalOut q_led_gre_fro (PTA5); //Led Green Front
ISR 0:15a30802e719 125 DigitalOut q_led_blu_fro (PTA12); //Led Blue Front
ISR 0:15a30802e719 126 DigitalOut q_led_red_rea (PTD4); //Led Red Rear
ISR 0:15a30802e719 127 DigitalOut q_led_gre_rea (PTA1); //Led Green Rear
ISR 0:15a30802e719 128 DigitalOut q_led_blu_rea (PTA2); //Led Blue Rear
ISR 0:15a30802e719 129
ISR 0:15a30802e719 130 //SAIDAS DIGITAIS (pwm)
ISR 0:15a30802e719 131 PwmOut pwm_mot_rig (PTE20); //PWM Enable Motor Right
ISR 0:15a30802e719 132 PwmOut pwm_mot_lef (PTE31); //PWM Enable Motor Left
ISR 0:15a30802e719 133 PwmOut pwm_buzzer (PTE21); //Buzzer PWM
ISR 0:15a30802e719 134 PwmOut pwm_led_whi (PTE29); //Led white pwm
ISR 0:15a30802e719 135
ISR 0:15a30802e719 136 // ********************************************************************
ISR 0:15a30802e719 137 //SAIDAS ANALOGICAS
ISR 0:15a30802e719 138 AnalogOut dac_comp_mic (PTE30); //Dac_Comparator MIC
ISR 0:15a30802e719 139
ISR 0:15a30802e719 140
ISR 1:8569ac717e68 141
ISR 0:15a30802e719 142
ISR 0:15a30802e719 143 /**
ISR 0:15a30802e719 144 * Selects the wich infrared to comunicate.
ISR 0:15a30802e719 145 *
ISR 0:15a30802e719 146 * @param ch - Infrared to read (1..5)
ISR 0:15a30802e719 147 */
ISR 0:15a30802e719 148 void tca9548_select_ch(char ch)
ISR 0:15a30802e719 149 {
ISR 0:15a30802e719 150 char ch_f[1];
ISR 0:15a30802e719 151 char addr=0xE0;
ISR 0:15a30802e719 152
ISR 0:15a30802e719 153 if(ch==0)
ISR 0:15a30802e719 154 ch_f[0]=1;
ISR 0:15a30802e719 155
ISR 0:15a30802e719 156 if(ch>=1)
ISR 0:15a30802e719 157 ch_f[0]=1<<ch;
ISR 0:15a30802e719 158
ISR 1:8569ac717e68 159 mutex_i2c0.lock();
ISR 0:15a30802e719 160 i2c.write(addr,ch_f,1);
ISR 1:8569ac717e68 161 mutex_i2c0.unlock();
ISR 1:8569ac717e68 162 }
ISR 1:8569ac717e68 163
ISR 1:8569ac717e68 164
ISR 1:8569ac717e68 165
ISR 1:8569ac717e68 166
ISR 1:8569ac717e68 167
ISR 1:8569ac717e68 168 /* Powers up all the VCNL4020. */
ISR 1:8569ac717e68 169 void init_Infrared()
ISR 1:8569ac717e68 170 {
ISR 1:8569ac717e68 171
ISR 1:8569ac717e68 172 for (int i=0; i<6;i++)
ISR 1:8569ac717e68 173 {
ISR 1:8569ac717e68 174 tca9548_select_ch(i);
ISR 1:8569ac717e68 175 VCNL40x0_Device.SetCurrent (20); // Set current to 200mA
ISR 1:8569ac717e68 176 }
ISR 1:8569ac717e68 177 tca9548_select_ch(0);
ISR 0:15a30802e719 178 }
ISR 0:15a30802e719 179
ISR 0:15a30802e719 180
ISR 0:15a30802e719 181 /**
ISR 1:8569ac717e68 182 * Get the distance from of the chosen infrared.
ISR 0:15a30802e719 183 *
ISR 1:8569ac717e68 184 * @param ch - Infrared to read (0,1..5)
ISR 0:15a30802e719 185 *
ISR 0:15a30802e719 186 * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
ISR 0:15a30802e719 187 */
ISR 1:8569ac717e68 188 float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
ISR 0:15a30802e719 189 {
ISR 0:15a30802e719 190 tca9548_select_ch(ch);
ISR 0:15a30802e719 191 VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue); // read prox value on demand
ISR 1:8569ac717e68 192 float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
ISR 1:8569ac717e68 193 return aux;
ISR 1:8569ac717e68 194 //return ProxiValue;
ISR 0:15a30802e719 195 }
ISR 0:15a30802e719 196
ISR 0:15a30802e719 197 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 198 /////////////////////////////////// MOTOR ///////////////////////////////////////////
ISR 0:15a30802e719 199 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 200
ISR 0:15a30802e719 201 // Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
ISR 0:15a30802e719 202 // consultar pag 39 e 95
ISR 0:15a30802e719 203
ISR 0:15a30802e719 204 /**
ISR 0:15a30802e719 205 * Sets speed and direction of the left motor.
ISR 0:15a30802e719 206 *
ISR 0:15a30802e719 207 * @param Dir - Direction of movement, 0 for back, or 1 for fron
ISR 0:15a30802e719 208 * @param Speed - Percentage of speed of the motor (1..100)
ISR 0:15a30802e719 209 *
ISR 0:15a30802e719 210 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
ISR 0:15a30802e719 211 * at different speeds and see if it makes a straigth line
ISR 0:15a30802e719 212 */
ISR 0:15a30802e719 213 void leftMotor(short int Dir,short int Speed)
ISR 0:15a30802e719 214 {
ISR 0:15a30802e719 215 float Duty;
ISR 0:15a30802e719 216
ISR 0:15a30802e719 217 if(Dir==1) {
ISR 0:15a30802e719 218 q_pha_mot_lef=0; //Andar em frente
ISR 0:15a30802e719 219 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 220 Speed=1000;
ISR 0:15a30802e719 221 if(Speed>0) {
ISR 0:15a30802e719 222 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 223 q_sleep_mot_lef=1; //Nano Sleep Motor Left
ISR 0:15a30802e719 224 pwm_mot_lef.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 225 } else {
ISR 0:15a30802e719 226 q_sleep_mot_lef=0;
ISR 0:15a30802e719 227 }
ISR 0:15a30802e719 228 }
ISR 0:15a30802e719 229 if(Dir==0) {
ISR 0:15a30802e719 230 q_pha_mot_lef=1; //Andar para tras
ISR 0:15a30802e719 231
ISR 0:15a30802e719 232 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 233 Speed=1000;
ISR 0:15a30802e719 234 if(Speed>0) {
ISR 0:15a30802e719 235 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 236 q_sleep_mot_lef=1; //Nano Sleep Motor Left
ISR 0:15a30802e719 237 pwm_mot_lef.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 238 } else {
ISR 0:15a30802e719 239 q_sleep_mot_lef=0;
ISR 0:15a30802e719 240 }
ISR 0:15a30802e719 241 }
ISR 0:15a30802e719 242 }
ISR 0:15a30802e719 243
ISR 0:15a30802e719 244
ISR 0:15a30802e719 245 /**
ISR 0:15a30802e719 246 * Sets speed and direction of the right motor.
ISR 0:15a30802e719 247 *
ISR 0:15a30802e719 248 * @param Dir - Direction of movement, 0 for back, or 1 for fron
ISR 0:15a30802e719 249 * @param Speed - Percentage of speed of the motor (1..100)
ISR 0:15a30802e719 250 *
ISR 0:15a30802e719 251 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
ISR 0:15a30802e719 252 * at different speeds and see if it makes a straigth line
ISR 0:15a30802e719 253 */
ISR 0:15a30802e719 254 void rightMotor(short int Dir,short int Speed)
ISR 0:15a30802e719 255 {
ISR 0:15a30802e719 256 float Duty;
ISR 0:15a30802e719 257
ISR 0:15a30802e719 258 if(Dir==1) {
ISR 0:15a30802e719 259 q_pha_mot_rig=0; //Andar em frente
ISR 0:15a30802e719 260
ISR 0:15a30802e719 261 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 262 Speed=1000;
ISR 0:15a30802e719 263 if(Speed>0) {
ISR 0:15a30802e719 264 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 265 q_sleep_mot_rig=1; //Nano Sleep Motor Right
ISR 0:15a30802e719 266 pwm_mot_rig.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 267 } else {
ISR 0:15a30802e719 268 q_sleep_mot_rig=0;
ISR 0:15a30802e719 269 }
ISR 0:15a30802e719 270 }
ISR 0:15a30802e719 271 if(Dir==0) {
ISR 0:15a30802e719 272 q_pha_mot_rig=1; //Andar para tras
ISR 0:15a30802e719 273
ISR 0:15a30802e719 274
ISR 0:15a30802e719 275 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 276 Speed=1000;
ISR 0:15a30802e719 277 if(Speed>0) {
ISR 0:15a30802e719 278 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 279 q_sleep_mot_rig=1; //Nano Sleep Motor Right
ISR 0:15a30802e719 280 pwm_mot_rig.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 281 } else {
ISR 0:15a30802e719 282 q_sleep_mot_rig=0;
ISR 0:15a30802e719 283 }
ISR 0:15a30802e719 284 }
ISR 0:15a30802e719 285 }
ISR 0:15a30802e719 286
ISR 0:15a30802e719 287
ISR 0:15a30802e719 288 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 289 /////////////////////////////////// BATTERY ///////////////////////////////////////////
ISR 0:15a30802e719 290 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 291
ISR 0:15a30802e719 292 /**
ISR 0:15a30802e719 293 * Reads adc of the battery.
ISR 0:15a30802e719 294 *
ISR 0:15a30802e719 295 * @param addr - Address to read
ISR 1:8569ac717e68 296 * @return The voltage of the baterry
ISR 0:15a30802e719 297 */
ISR 0:15a30802e719 298 long int read16_mcp3424(char addr)
ISR 0:15a30802e719 299 {
ISR 0:15a30802e719 300 char data[4];
ISR 1:8569ac717e68 301 mutex_i2c1.lock();
ISR 0:15a30802e719 302 i2c1.read(addr,data,3);
ISR 1:8569ac717e68 303 mutex_i2c1.unlock();
ISR 0:15a30802e719 304 return(((data[0]&127)*256)+data[1]);
ISR 0:15a30802e719 305 }
ISR 0:15a30802e719 306
ISR 0:15a30802e719 307 /**
ISR 0:15a30802e719 308 * Reads adc of the battery.
ISR 0:15a30802e719 309 *
ISR 0:15a30802e719 310 * @param n_bits - Resolution of measure
ISR 0:15a30802e719 311 * @param ch - Chose value to read, if voltage or current of solar or batery
ISR 0:15a30802e719 312 * @param gain -
ISR 0:15a30802e719 313 * @param addr - Address to write to
ISR 0:15a30802e719 314 */
ISR 0:15a30802e719 315 void write_mcp3424(int n_bits, int ch, int gain, char addr) //chanel 1-4 write -> 0xD0
ISR 0:15a30802e719 316 {
ISR 0:15a30802e719 317
ISR 0:15a30802e719 318 int chanel_end=(ch-1)<<5; //shift left
ISR 0:15a30802e719 319 char n_bits_end=0;
ISR 0:15a30802e719 320
ISR 0:15a30802e719 321 if(n_bits==12) {
ISR 0:15a30802e719 322 n_bits_end=0;
ISR 0:15a30802e719 323 } else if(n_bits==14) {
ISR 0:15a30802e719 324 n_bits_end=1;
ISR 0:15a30802e719 325 } else if(n_bits==16) {
ISR 0:15a30802e719 326 n_bits_end=2;
ISR 0:15a30802e719 327 } else {
ISR 0:15a30802e719 328 n_bits_end=3;
ISR 0:15a30802e719 329 }
ISR 0:15a30802e719 330 n_bits_end=n_bits_end<<2; //shift left
ISR 0:15a30802e719 331
ISR 0:15a30802e719 332 char data[1];
ISR 0:15a30802e719 333 data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
ISR 1:8569ac717e68 334 mutex_i2c1.lock();
ISR 0:15a30802e719 335 i2c1.write(addr,data,1);
ISR 1:8569ac717e68 336 mutex_i2c1.unlock();
ISR 0:15a30802e719 337 }
ISR 0:15a30802e719 338
ISR 0:15a30802e719 339
ISR 0:15a30802e719 340 /**
ISR 0:15a30802e719 341 * Reads adc of the battery.
ISR 0:15a30802e719 342 *
ISR 0:15a30802e719 343 * @return The voltage of the batery
ISR 0:15a30802e719 344 */
ISR 0:15a30802e719 345 float value_of_Batery()
ISR 0:15a30802e719 346 {
ISR 0:15a30802e719 347 float R1=75000.0;
ISR 0:15a30802e719 348 float R2=39200.0;
ISR 0:15a30802e719 349 float R3=178000.0;
ISR 0:15a30802e719 350 float Gain=1.0;
ISR 0:15a30802e719 351 write_mcp3424(16,3,1,0xd8);
ISR 0:15a30802e719 352 float cha3_v2=read16_mcp3424(0xd9); //read voltage
ISR 0:15a30802e719 353 float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
ISR 0:15a30802e719 354 float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
ISR 0:15a30802e719 355 Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
ISR 0:15a30802e719 356
ISR 0:15a30802e719 357 return Vin_b_v_battery;
ISR 0:15a30802e719 358 }
ISR 0:15a30802e719 359
ISR 0:15a30802e719 360 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 361 ////////////////////////////////// Sonar ////////////////////////////////////////////
ISR 0:15a30802e719 362 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 363 // Commands of operation with ultrasonic module
ISR 0:15a30802e719 364
ISR 0:15a30802e719 365 // WRITE OPTION:
ISR 0:15a30802e719 366 // ENABLE DC DC CONVERTER - 0x0C;
ISR 0:15a30802e719 367 // DISABLE DC DC CONVERTER - 0x0B;
ISR 0:15a30802e719 368 // START MEASURE LEFT SENSOR - 0x0A;
ISR 0:15a30802e719 369 // START MEASURE FRONT SENSOR - 0x09;
ISR 0:15a30802e719 370 // START MEASURE RIGHT SENSOR - 0x08;
ISR 0:15a30802e719 371 // SENSORS ALWAYS MEASURE ON - 0x07;
ISR 0:15a30802e719 372 // SENSORS ALWAYS MEASURE OFF - 0x06;
ISR 0:15a30802e719 373
ISR 0:15a30802e719 374 // READ OPTION:
ISR 0:15a30802e719 375 // GET MEASURE OF LEFT SENSOR - 0x05;
ISR 0:15a30802e719 376 // GET MEASURE OF FRONT SENSOR - 0x04;
ISR 0:15a30802e719 377 // GET MEASURE OF IGHT SENSOR - 0x03;
ISR 0:15a30802e719 378 // GET STATUS SENSORS ALWAYS MEASURE - 0x02;
ISR 0:15a30802e719 379 // GET STATUS DC DC CONVERTER - 0x01;
ISR 0:15a30802e719 380
ISR 0:15a30802e719 381 void enable_dc_dc_boost()
ISR 0:15a30802e719 382 {
ISR 0:15a30802e719 383 char data[1];
ISR 0:15a30802e719 384 data[0]= 0x0C;
ISR 1:8569ac717e68 385 mutex_i2c1.lock();
ISR 0:15a30802e719 386 i2c1.write(0x30,data,1);
ISR 1:8569ac717e68 387 mutex_i2c1.unlock();
ISR 0:15a30802e719 388 }
ISR 0:15a30802e719 389
ISR 0:15a30802e719 390
ISR 0:15a30802e719 391 void measure_always_on() // left, front, right
ISR 0:15a30802e719 392 {
ISR 0:15a30802e719 393 char data[1];
ISR 0:15a30802e719 394 data[0]= 0x07;
ISR 1:8569ac717e68 395 mutex_i2c1.lock();
ISR 0:15a30802e719 396 i2c1.write(0x30,data,1);
ISR 1:8569ac717e68 397 mutex_i2c1.unlock();
ISR 0:15a30802e719 398 }
ISR 0:15a30802e719 399
ISR 0:15a30802e719 400
ISR 0:15a30802e719 401 /**
ISR 0:15a30802e719 402 * Returns left sensor value
ISR 0:15a30802e719 403 */
ISR 0:15a30802e719 404 static unsigned int get_distance_left_sensor()
ISR 0:15a30802e719 405 {
ISR 1:8569ac717e68 406
ISR 0:15a30802e719 407 static char data_r[3];
ISR 0:15a30802e719 408 static unsigned int aux;
ISR 1:8569ac717e68 409
ISR 0:15a30802e719 410 data_r[0]= 0x05;
ISR 1:8569ac717e68 411 mutex_i2c1.lock();
ISR 0:15a30802e719 412 i2c1.write(0x30,data_r,1);
ISR 1:8569ac717e68 413 i2c1.read(0x31,data_r,2);
ISR 1:8569ac717e68 414 mutex_i2c1.unlock();
ISR 0:15a30802e719 415
ISR 0:15a30802e719 416 aux=(data_r[0]*256)+data_r[1];
ISR 1:8569ac717e68 417
ISR 0:15a30802e719 418 return aux;
ISR 0:15a30802e719 419 }
ISR 0:15a30802e719 420
ISR 0:15a30802e719 421
ISR 0:15a30802e719 422 /**
ISR 0:15a30802e719 423 * Returns front sensor value
ISR 0:15a30802e719 424 */
ISR 0:15a30802e719 425 static unsigned int get_distance_front_sensor()
ISR 0:15a30802e719 426 {
ISR 0:15a30802e719 427
ISR 0:15a30802e719 428 static char data_r[3];
ISR 0:15a30802e719 429 static unsigned int aux;
ISR 1:8569ac717e68 430
ISR 0:15a30802e719 431 data_r[0]= 0x04;
ISR 1:8569ac717e68 432
ISR 1:8569ac717e68 433 mutex_i2c1.lock();
ISR 0:15a30802e719 434 i2c1.write(0x30,data_r,1);
ISR 1:8569ac717e68 435 i2c1.read(0x31,data_r,2);
ISR 1:8569ac717e68 436 mutex_i2c1.unlock();
ISR 0:15a30802e719 437
ISR 0:15a30802e719 438 aux=(data_r[0]*256)+data_r[1];
ISR 1:8569ac717e68 439
ISR 0:15a30802e719 440 return aux;
ISR 0:15a30802e719 441
ISR 0:15a30802e719 442 }
ISR 0:15a30802e719 443
ISR 0:15a30802e719 444
ISR 0:15a30802e719 445 /**
ISR 0:15a30802e719 446 * Returns right sensor value
ISR 0:15a30802e719 447 */
ISR 0:15a30802e719 448 static unsigned int get_distance_right_sensor()
ISR 0:15a30802e719 449 {
ISR 0:15a30802e719 450
ISR 0:15a30802e719 451 static char data_r[3];
ISR 0:15a30802e719 452 static unsigned int aux;
ISR 0:15a30802e719 453
ISR 0:15a30802e719 454 data_r[0]= 0x03;
ISR 1:8569ac717e68 455
ISR 1:8569ac717e68 456 mutex_i2c1.lock();
ISR 0:15a30802e719 457 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 458 i2c1.read(0x31,data_r,2,0);
ISR 1:8569ac717e68 459 mutex_i2c1.unlock();
ISR 0:15a30802e719 460
ISR 0:15a30802e719 461 aux=(data_r[0]*256)+data_r[1];
ISR 1:8569ac717e68 462
ISR 0:15a30802e719 463 return aux;
ISR 0:15a30802e719 464
ISR 0:15a30802e719 465 }
ISR 0:15a30802e719 466
ISR 0:15a30802e719 467
ISR 0:15a30802e719 468
ISR 0:15a30802e719 469 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 470 ////////////////////////////////// MISC. ////////////////////////////////////////////
ISR 0:15a30802e719 471 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 472
ISR 0:15a30802e719 473
ISR 0:15a30802e719 474 /**
ISR 0:15a30802e719 475 * Initializes the necessary robot pins
ISR 0:15a30802e719 476 */
ISR 0:15a30802e719 477 void init_robot_pins()
ISR 0:15a30802e719 478 {
ISR 0:15a30802e719 479
ISR 0:15a30802e719 480 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 481 //q_pha_mot_rig=0; //Phase Motor Right
ISR 0:15a30802e719 482 //q_sleep_mot_rig=0; //Nano Sleep Motor Right
ISR 0:15a30802e719 483 //q_pha_mot_lef=0; //Phase Motor Left
ISR 0:15a30802e719 484 //q_sleep_mot_lef=0; //Nano Sleep Motor Left
ISR 0:15a30802e719 485 //q_pow_ena_i2c_p=0; //Power Enable i2c FET P
ISR 0:15a30802e719 486 //q_pow_ena_mic_p=0; //Power enable Micro FET P
ISR 0:15a30802e719 487 //q_pow_as5600_n=1; //AS5600 Power MOSFET N
ISR 0:15a30802e719 488 //q_pow_as5600_p=0; //AS5600 Power MOSFET P
ISR 0:15a30802e719 489 //q_pow_spi=0; //SPI Power MOSFET P
ISR 0:15a30802e719 490 //q_ena_mppt=0; //Enable MPPT Control
ISR 0:15a30802e719 491 //q_boost_ps=1; //Boost Power Save
ISR 0:15a30802e719 492 //q_tca9548_reset=1; //Reset TCA9548
ISR 0:15a30802e719 493
ISR 0:15a30802e719 494 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 495 q_pha_mot_rig=0; //Phase Motor Right
ISR 0:15a30802e719 496 q_sleep_mot_rig=0; //Nano Sleep Motor Right
ISR 0:15a30802e719 497 q_pha_mot_lef=0; //Phase Motor Left
ISR 0:15a30802e719 498 q_sleep_mot_lef=0; //Nano Sleep Motor Left
ISR 0:15a30802e719 499
ISR 0:15a30802e719 500 q_pow_ena_i2c_p=0; //Power Enable i2c FET P
ISR 0:15a30802e719 501 q_pow_ena_mic_p=0; //Power enable Micro FET P
ISR 0:15a30802e719 502 q_pow_as5600_p=0; //AS5600 Power MOSFET P
ISR 0:15a30802e719 503 // q_pow_spi=0; //SPI Power MOSFET P
ISR 0:15a30802e719 504 q_pow_as5600_n=1; //AS5600 Power MOSFET N
ISR 0:15a30802e719 505
ISR 0:15a30802e719 506
ISR 0:15a30802e719 507 q_ena_mppt=0; //Enable MPPT Control
ISR 0:15a30802e719 508 q_boost_ps=1; //Boost Power Save
ISR 0:15a30802e719 509 q_tca9548_reset=1; //Reset TCA9548
ISR 0:15a30802e719 510
ISR 0:15a30802e719 511 //Leds caso seja saida digital:
ISR 0:15a30802e719 512 q_led_red_fro=1; //Led Red Front (led off)
ISR 0:15a30802e719 513 q_led_gre_fro=1; //Led Green Front (led off)
ISR 0:15a30802e719 514 q_led_blu_fro=1; //Led Blue Front (led off)
ISR 0:15a30802e719 515 q_led_red_rea=1; //Led Red Rear (led off)
ISR 0:15a30802e719 516 q_led_gre_rea=1; //Led Green Rear (led off)
ISR 0:15a30802e719 517 q_led_blu_rea=1; //Led Blue Rear (led off)r
ISR 0:15a30802e719 518
ISR 0:15a30802e719 519
ISR 0:15a30802e719 520 //********************************************************************
ISR 0:15a30802e719 521 //SAIDAS DIGITAIS (pwm)
ISR 0:15a30802e719 522 //PWM Enable Motor Right
ISR 0:15a30802e719 523 pwm_mot_rig.period_us(500);
ISR 0:15a30802e719 524 pwm_mot_rig.pulsewidth_us(0);
ISR 0:15a30802e719 525
ISR 0:15a30802e719 526 //PWM Enable Motor Left
ISR 0:15a30802e719 527 pwm_mot_lef.period_us(500);
ISR 0:15a30802e719 528 pwm_mot_lef.pulsewidth_us(0);
ISR 0:15a30802e719 529
ISR 0:15a30802e719 530 //Buzzer PWM
ISR 0:15a30802e719 531 pwm_buzzer.period_us(500);
ISR 0:15a30802e719 532 pwm_buzzer.pulsewidth_us(0);
ISR 0:15a30802e719 533
ISR 1:8569ac717e68 534
ISR 0:15a30802e719 535 //LED white
ISR 0:15a30802e719 536 pwm_led_whi.period_us(500);
ISR 0:15a30802e719 537 pwm_led_whi.pulsewidth_us(0);
ISR 0:15a30802e719 538
ISR 0:15a30802e719 539 }
ISR 0:15a30802e719 540
ISR 0:15a30802e719 541 /**
ISR 0:15a30802e719 542 * Initializes all the pins and all the modules necessary
ISR 1:8569ac717e68 543 *
ISR 1:8569ac717e68 544 * @param channel - Which RF channel to comunicate on, 0-125.
ISR 1:8569ac717e68 545 *\note If you are not using RF module put random number.
ISR 1:8569ac717e68 546 * \warning Channel on Robot and Board has to be the same.
ISR 0:15a30802e719 547 */
ISR 1:8569ac717e68 548 void initRobot(int channel)
ISR 1:8569ac717e68 549 {
ISR 1:8569ac717e68 550 pc.printf("Battery level: \n\r");
ISR 1:8569ac717e68 551 init_nRF(channel);
ISR 0:15a30802e719 552 init_robot_pins();
ISR 3:215c9544480d 553
ISR 0:15a30802e719 554 enable_dc_dc_boost();
ISR 1:8569ac717e68 555 wait_ms(100); //wait for read wait(>=150ms);
ISR 0:15a30802e719 556 enable_dc_dc_boost();
ISR 1:8569ac717e68 557 measure_always_on();
ISR 0:15a30802e719 558 wait_ms(100); //wait for read wait(>=150ms);
ISR 1:8569ac717e68 559
ISR 1:8569ac717e68 560 init_Infrared();
ISR 3:215c9544480d 561
fabiofaria 4:f6fddeae358e 562 init_rangefinder(&i2c1, &mutex_i2c1);
fabiofaria 4:f6fddeae358e 563
ISR 1:8569ac717e68 564
ISR 1:8569ac717e68 565 thread.start(odometry_thread);
ISR 1:8569ac717e68 566
ISR 3:215c9544480d 567 float value = value_of_Batery();
ISR 3:215c9544480d 568
ISR 0:15a30802e719 569 pc.printf("Initialization Successful \n\r");
ISR 0:15a30802e719 570 pc.printf("Battery level: %f \n\r",value);
ISR 0:15a30802e719 571 if(value < 3.0) {
ISR 0:15a30802e719 572 pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
ISR 0:15a30802e719 573 }
ISR 0:15a30802e719 574 // float level = value_of_Batery();
ISR 0:15a30802e719 575 // sendValue(int(level*100));
ISR 0:15a30802e719 576
ISR 0:15a30802e719 577 }