Fabio Faria / ISR_Mini-explorer_tmp

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

Dependencies:   RF24

Fork of ISR_Mini-explorer by ISR UC

robot.h@3:215c9544480d, 2018-03-16 (annotated)

Committer:
ISR
Date:
Fri Mar 16 16:49:02 2018 +0000
Revision:
3:215c9544480d
Parent:
2:0435d1171673
Child:
4:560d24c0e5f8
Radio communication added

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