Fabio Faria / ISR_Mini-explorer

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

Fork of ISR_Mini-explorer by ISR UC

robot.h@1:d1443589406e, 2017-07-27 (annotated)

Committer:
fabiofaria
Date:
Thu Jul 27 13:05:03 2017 +0000
Revision:
1:d1443589406e
Parent:
0:15a30802e719 
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ISR 0:15a30802e719 1 /** @file */
ISR 0:15a30802e719 2 //AUTOR: Fernando Pais
ISR 0:15a30802e719 3 //MAIL: ferpais2508@gmail.com
ISR 0:15a30802e719 4 //DATA: 6/6/2016
ISR 0:15a30802e719 5 // VERSÃO 6.4.0
ISR 0:15a30802e719 6 //
ISR 0:15a30802e719 7 //Alterações: Problema de compatibilidade entre encoder e infravermelho resolvido
ISR 0:15a30802e719 8 // Odometria actualizada automaticamente
ISR 0:15a30802e719 9 // Valor da bateria verificado na inicialização
ISR 0:15a30802e719 10 // Motores movem-se de 0 a 1000 para melhor difrenciação
ISR 0:15a30802e719 11 //
ISR 0:15a30802e719 12
ISR 0:15a30802e719 13 //#include "mbed.h"
ISR 0:15a30802e719 14 //#include "init.h"
ISR 0:15a30802e719 15 //#define _USE_MATH_DEFINES
ISR 0:15a30802e719 16 # define M_PI 3.14159265358979323846 /* pi */
ISR 0:15a30802e719 17 #include <math.h>
ISR 0:15a30802e719 18 #include <string.h>
ISR 0:15a30802e719 19 #include "VCNL40x0.h"
ISR 0:15a30802e719 20
ISR 0:15a30802e719 21 void Odometria();
ISR 0:15a30802e719 22
ISR 0:15a30802e719 23 // classes adicionais
fabiofaria 1:d1443589406e 24
ISR 0:15a30802e719 25 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
ISR 0:15a30802e719 26 Timeout timeout;
ISR 0:15a30802e719 27
ISR 0:15a30802e719 28 Serial pc(PTE0,PTE1);
ISR 0:15a30802e719 29 I2C i2c(PTC9,PTC8);
ISR 0:15a30802e719 30 I2C i2c1(PTC11,PTC10);
ISR 0:15a30802e719 31
ISR 0:15a30802e719 32 // Variables needed by the lib
ISR 0:15a30802e719 33 unsigned int ProxiValue=0;
ISR 0:15a30802e719 34 short int prev_R=0;
ISR 0:15a30802e719 35 short int prev_L=0;
ISR 0:15a30802e719 36 long int total_R=0;
ISR 0:15a30802e719 37 long int total_L=0;
ISR 0:15a30802e719 38 long int ticks2d=0;
ISR 0:15a30802e719 39 long int ticks2e=0;
ISR 0:15a30802e719 40 float X=20;
ISR 0:15a30802e719 41 float Y=20;
ISR 0:15a30802e719 42 float AtractX = 0;
ISR 0:15a30802e719 43 float AtractY = 0;
ISR 0:15a30802e719 44 float theta=0;
ISR 0:15a30802e719 45 int sensor_left=0;
ISR 0:15a30802e719 46 int sensor_front=0;
ISR 0:15a30802e719 47 int sensor_right=0;
ISR 0:15a30802e719 48 short int flag=0;
ISR 0:15a30802e719 49 int IRobot=0;
ISR 0:15a30802e719 50 int JRobot=0;
ISR 0:15a30802e719 51
ISR 0:15a30802e719 52 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 53 DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right
ISR 0:15a30802e719 54 DigitalOut q_sleep_mot_rig (PTE3,0); //Nano Sleep Motor Right
ISR 0:15a30802e719 55 DigitalOut q_pha_mot_lef (PTA17,0); //Phase Motor Left
ISR 0:15a30802e719 56 DigitalOut q_sleep_mot_lef (PTB11,0); //Nano Sleep Motor Left
ISR 0:15a30802e719 57 DigitalOut q_pow_ena_i2c_p (PTE2,0); //Power Enable i2c FET P (0- enable 1-disable)
ISR 0:15a30802e719 58 DigitalOut q_pow_ena_mic_p (PTA14,0); //Power enable Micro FET P (0- enable 1-disable)
ISR 0:15a30802e719 59 DigitalOut q_pow_as5600_n (PTC6,1); //AS5600 Power MOSFET N (1- enable 0-disable)
ISR 0:15a30802e719 60 DigitalOut q_pow_as5600_p (PTC5,0); //AS5600 Power MOSFET P (0- enable 1-disable)
ISR 0:15a30802e719 61 DigitalOut q_pow_spi (PTC4,0); //SPI Power MOSFET P (0- enable 1-disable)
ISR 0:15a30802e719 62 DigitalOut q_ena_mppt (PTC0,0); //Enable MPPT Control (0- enable 1-disable)
ISR 0:15a30802e719 63 DigitalOut q_boost_ps (PTC7,1); //Boost Power Save (1- enable 0-disable)
ISR 0:15a30802e719 64 DigitalOut q_tca9548_reset (PTC3,1); //Reset TCA9548 (1- enable 0-disable)
ISR 0:15a30802e719 65 DigitalOut power_36khz (PTD0,0); //Power enable pic12f - 36khz (0- enable 1-disable)
ISR 0:15a30802e719 66
ISR 0:15a30802e719 67
ISR 0:15a30802e719 68 // ********************************************************************
ISR 0:15a30802e719 69 // ********************************************************************
ISR 0:15a30802e719 70 //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
ISR 0:15a30802e719 71 //ENTRADAS DIGITAIS (normal input)
ISR 0:15a30802e719 72 DigitalIn i_enc_dir_rig (PTB8); //Encoder Right Direction
ISR 0:15a30802e719 73 DigitalIn i_enc_dir_lef (PTB9); //Encoder Left Direction
ISR 0:15a30802e719 74 DigitalIn i_micro_sd_det (PTC16); //MICRO SD Card Detect
ISR 0:15a30802e719 75 DigitalIn i_mppt_fail (PTE5); //Fail MPPT Signal
ISR 0:15a30802e719 76 DigitalIn i_usb_volt (PTB10); //USB Voltage detect
ISR 0:15a30802e719 77 DigitalIn i_sup_cap_est (PTB19); //Supercap State Charger
ISR 0:15a30802e719 78 DigitalIn i_li_ion_est (PTB18); //Li-ion State Charger
ISR 0:15a30802e719 79
ISR 0:15a30802e719 80
ISR 0:15a30802e719 81 // ********************************************************************
ISR 0:15a30802e719 82 //ENTRADAS DIGITAIS (external interrupt)
ISR 0:15a30802e719 83 InterruptIn i_int_mpu9250 (PTA15); //Interrupt MPU9250
ISR 0:15a30802e719 84 InterruptIn i_int_isl29125 (PTA16); //Interrupt ISL29125 Color S.
ISR 0:15a30802e719 85 InterruptIn i_mic_f_l (PTD7); //Interrupt Comp Micro F L
ISR 0:15a30802e719 86 InterruptIn i_mic_f_r (PTD6); //Interrupt Comp Micro F R
ISR 0:15a30802e719 87 InterruptIn i_mic_r_c (PTD5); //Interrupt Comp Micro R C
ISR 0:15a30802e719 88
ISR 0:15a30802e719 89
ISR 0:15a30802e719 90 // ********************************************************************
ISR 0:15a30802e719 91 //ENTRADAS ANALOGICAS
ISR 0:15a30802e719 92 AnalogIn a_enc_rig (PTC2); //Encoder Left Output_AS_MR
ISR 0:15a30802e719 93 AnalogIn a_enc_lef (PTC1); //Encoder Right Output_AS_ML
ISR 0:15a30802e719 94 AnalogIn a_mic_f_l (PTB0); //Analog microphone F L
ISR 0:15a30802e719 95 AnalogIn a_mic_f_r (PTB1); //Analog microphone F R
ISR 0:15a30802e719 96 AnalogIn a_mic_r_c (PTB2); //Analog microphone R C
ISR 0:15a30802e719 97 AnalogIn a_temp_bat (PTB3); //Temperature Battery
ISR 0:15a30802e719 98
ISR 0:15a30802e719 99
ISR 0:15a30802e719 100 // ********************************************************************
ISR 0:15a30802e719 101
ISR 0:15a30802e719 102 //PWM OR DIGITAL OUTPUT NORMAL
ISR 0:15a30802e719 103 //DigitalOut q_led_whi (PTE29); //Led white pwm
ISR 0:15a30802e719 104 DigitalOut q_led_red_fro (PTA4); //Led Red Front
ISR 0:15a30802e719 105 DigitalOut q_led_gre_fro (PTA5); //Led Green Front
ISR 0:15a30802e719 106 DigitalOut q_led_blu_fro (PTA12); //Led Blue Front
ISR 0:15a30802e719 107 DigitalOut q_led_red_rea (PTD4); //Led Red Rear
ISR 0:15a30802e719 108 DigitalOut q_led_gre_rea (PTA1); //Led Green Rear
ISR 0:15a30802e719 109 DigitalOut q_led_blu_rea (PTA2); //Led Blue Rear
ISR 0:15a30802e719 110
ISR 0:15a30802e719 111
ISR 0:15a30802e719 112 //SAIDAS DIGITAIS (pwm)
ISR 0:15a30802e719 113 PwmOut pwm_mot_rig (PTE20); //PWM Enable Motor Right
ISR 0:15a30802e719 114 PwmOut pwm_mot_lef (PTE31); //PWM Enable Motor Left
ISR 0:15a30802e719 115 PwmOut pwm_buzzer (PTE21); //Buzzer PWM
ISR 0:15a30802e719 116 PwmOut pwm_led_whi (PTE29); //Led white pwm
ISR 0:15a30802e719 117
ISR 0:15a30802e719 118 // ********************************************************************
ISR 0:15a30802e719 119 //SAIDAS ANALOGICAS
ISR 0:15a30802e719 120 AnalogOut dac_comp_mic (PTE30); //Dac_Comparator MIC
ISR 0:15a30802e719 121
ISR 0:15a30802e719 122
ISR 0:15a30802e719 123 /* Powers up all the VCNL4020. */
ISR 0:15a30802e719 124 void init_Infrared()
ISR 0:15a30802e719 125 {
ISR 0:15a30802e719 126 VCNL40x0_Device.SetCurrent (20); // Set current to 200mA
ISR 0:15a30802e719 127 }
ISR 0:15a30802e719 128
ISR 0:15a30802e719 129 /**
ISR 0:15a30802e719 130 * Selects the wich infrared to comunicate.
ISR 0:15a30802e719 131 *
ISR 0:15a30802e719 132 * @param ch - Infrared to read (1..5)
ISR 0:15a30802e719 133 */
ISR 0:15a30802e719 134 void tca9548_select_ch(char ch)
ISR 0:15a30802e719 135 {
ISR 0:15a30802e719 136 char ch_f[1];
ISR 0:15a30802e719 137 char addr=0xE0;
ISR 0:15a30802e719 138
ISR 0:15a30802e719 139 if(ch==0)
ISR 0:15a30802e719 140 ch_f[0]=1;
ISR 0:15a30802e719 141
ISR 0:15a30802e719 142 if(ch>=1)
ISR 0:15a30802e719 143 ch_f[0]=1<<ch;
ISR 0:15a30802e719 144
ISR 0:15a30802e719 145 i2c.start();
ISR 0:15a30802e719 146 i2c.write(addr,ch_f,1);
ISR 0:15a30802e719 147 i2c.stop();
ISR 0:15a30802e719 148 }
ISR 0:15a30802e719 149
ISR 0:15a30802e719 150
ISR 0:15a30802e719 151 /**
ISR 0:15a30802e719 152 * Get ADC value of the chosen infrared.
ISR 0:15a30802e719 153 *
ISR 0:15a30802e719 154 * @param ch - Infrared to read (1..5)
ISR 0:15a30802e719 155 *
ISR 0:15a30802e719 156 * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
ISR 0:15a30802e719 157 */
ISR 0:15a30802e719 158 long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
ISR 0:15a30802e719 159 {
ISR 0:15a30802e719 160 tca9548_select_ch(ch);
ISR 0:15a30802e719 161 VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue); // read prox value on demand
ISR 0:15a30802e719 162
ISR 0:15a30802e719 163 return ProxiValue;
ISR 0:15a30802e719 164 }
ISR 0:15a30802e719 165
ISR 0:15a30802e719 166 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 167 /////////////////////////////////// MOTOR ///////////////////////////////////////////
ISR 0:15a30802e719 168 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 169
ISR 0:15a30802e719 170 // 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 171 // consultar pag 39 e 95
ISR 0:15a30802e719 172
ISR 0:15a30802e719 173 /**
ISR 0:15a30802e719 174 * Sets speed and direction of the left motor.
ISR 0:15a30802e719 175 *
ISR 0:15a30802e719 176 * @param Dir - Direction of movement, 0 for back, or 1 for fron
ISR 0:15a30802e719 177 * @param Speed - Percentage of speed of the motor (1..100)
ISR 0:15a30802e719 178 *
ISR 0:15a30802e719 179 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
ISR 0:15a30802e719 180 * at different speeds and see if it makes a straigth line
ISR 0:15a30802e719 181 */
ISR 0:15a30802e719 182 void leftMotor(short int Dir,short int Speed)
ISR 0:15a30802e719 183 {
ISR 0:15a30802e719 184 float Duty;
ISR 0:15a30802e719 185
ISR 0:15a30802e719 186 if(Dir==1) {
ISR 0:15a30802e719 187 q_pha_mot_lef=0; //Andar em frente
ISR 0:15a30802e719 188 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 189 Speed=1000;
ISR 0:15a30802e719 190 if(Speed>0) {
ISR 0:15a30802e719 191 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 192 q_sleep_mot_lef=1; //Nano Sleep Motor Left
ISR 0:15a30802e719 193 pwm_mot_lef.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 194 } else {
ISR 0:15a30802e719 195 q_sleep_mot_lef=0;
ISR 0:15a30802e719 196 }
ISR 0:15a30802e719 197 }
ISR 0:15a30802e719 198 if(Dir==0) {
ISR 0:15a30802e719 199 q_pha_mot_lef=1; //Andar para tras
ISR 0:15a30802e719 200
ISR 0:15a30802e719 201 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 202 Speed=1000;
ISR 0:15a30802e719 203 if(Speed>0) {
ISR 0:15a30802e719 204 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 205 q_sleep_mot_lef=1; //Nano Sleep Motor Left
ISR 0:15a30802e719 206 pwm_mot_lef.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 207 } else {
ISR 0:15a30802e719 208 q_sleep_mot_lef=0;
ISR 0:15a30802e719 209 }
ISR 0:15a30802e719 210 }
ISR 0:15a30802e719 211 }
ISR 0:15a30802e719 212
ISR 0:15a30802e719 213
ISR 0:15a30802e719 214 /**
ISR 0:15a30802e719 215 * Sets speed and direction of the right motor.
ISR 0:15a30802e719 216 *
ISR 0:15a30802e719 217 * @param Dir - Direction of movement, 0 for back, or 1 for fron
ISR 0:15a30802e719 218 * @param Speed - Percentage of speed of the motor (1..100)
ISR 0:15a30802e719 219 *
ISR 0:15a30802e719 220 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
ISR 0:15a30802e719 221 * at different speeds and see if it makes a straigth line
ISR 0:15a30802e719 222 */
ISR 0:15a30802e719 223 void rightMotor(short int Dir,short int Speed)
ISR 0:15a30802e719 224 {
ISR 0:15a30802e719 225 float Duty;
ISR 0:15a30802e719 226
ISR 0:15a30802e719 227 if(Dir==1) {
ISR 0:15a30802e719 228 q_pha_mot_rig=0; //Andar em frente
ISR 0:15a30802e719 229
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_rig=1; //Nano Sleep Motor Right
ISR 0:15a30802e719 235 pwm_mot_rig.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 236 } else {
ISR 0:15a30802e719 237 q_sleep_mot_rig=0;
ISR 0:15a30802e719 238 }
ISR 0:15a30802e719 239 }
ISR 0:15a30802e719 240 if(Dir==0) {
ISR 0:15a30802e719 241 q_pha_mot_rig=1; //Andar para tras
ISR 0:15a30802e719 242
ISR 0:15a30802e719 243
ISR 0:15a30802e719 244 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 245 Speed=1000;
ISR 0:15a30802e719 246 if(Speed>0) {
ISR 0:15a30802e719 247 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 248 q_sleep_mot_rig=1; //Nano Sleep Motor Right
ISR 0:15a30802e719 249 pwm_mot_rig.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 250 } else {
ISR 0:15a30802e719 251 q_sleep_mot_rig=0;
ISR 0:15a30802e719 252 }
ISR 0:15a30802e719 253 }
ISR 0:15a30802e719 254 }
ISR 0:15a30802e719 255
ISR 0:15a30802e719 256
ISR 0:15a30802e719 257 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 258 /////////////////////////////////// ENCODER ///////////////////////////////////////////
ISR 0:15a30802e719 259 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 260
ISR 0:15a30802e719 261 /**
ISR 0:15a30802e719 262 * Reads Position of left magnetic encoder.
ISR 0:15a30802e719 263 *
ISR 0:15a30802e719 264 * @return The absolute position of the left wheel encoder (0..4095)
ISR 0:15a30802e719 265 */
ISR 0:15a30802e719 266 long int read_L_encoder()
ISR 0:15a30802e719 267 {
ISR 0:15a30802e719 268
ISR 0:15a30802e719 269 char data_r_2[5];
ISR 0:15a30802e719 270
ISR 0:15a30802e719 271 i2c.start();
ISR 0:15a30802e719 272 i2c.write(0x6C);
ISR 0:15a30802e719 273 i2c.write(0x0C);
ISR 0:15a30802e719 274 i2c.read(0x6D,data_r_2,4,0);
ISR 0:15a30802e719 275 i2c.stop();
ISR 0:15a30802e719 276
ISR 0:15a30802e719 277 short int val1=data_r_2[0];
ISR 0:15a30802e719 278 short int val2=data_r_2[1];
ISR 0:15a30802e719 279 val1=(val1&0xf)*256;
ISR 0:15a30802e719 280 long int final=(val2+val1);
ISR 0:15a30802e719 281
ISR 0:15a30802e719 282 return final;
ISR 0:15a30802e719 283 }
ISR 0:15a30802e719 284
ISR 0:15a30802e719 285
ISR 0:15a30802e719 286 /**
ISR 0:15a30802e719 287 * Reads Position of right magnetic encoder.
ISR 0:15a30802e719 288 *
ISR 0:15a30802e719 289 * @return The absolute position of the right wheel encoder (0..4095)
ISR 0:15a30802e719 290 */
ISR 0:15a30802e719 291 long int read_R_encoder()
ISR 0:15a30802e719 292 {
ISR 0:15a30802e719 293
ISR 0:15a30802e719 294 char data_r_2[5];
ISR 0:15a30802e719 295
ISR 0:15a30802e719 296 i2c1.start();
ISR 0:15a30802e719 297 i2c1.write(0x6C);
ISR 0:15a30802e719 298 i2c1.write(0x0C);
ISR 0:15a30802e719 299 i2c1.read(0x6D,data_r_2,4,0);
ISR 0:15a30802e719 300 i2c1.stop();
ISR 0:15a30802e719 301
ISR 0:15a30802e719 302 short int val1=data_r_2[0];
ISR 0:15a30802e719 303 short int val2=data_r_2[1];
ISR 0:15a30802e719 304 val1=(val1&0xf)*256;
ISR 0:15a30802e719 305 long int final=(val2+val1);
ISR 0:15a30802e719 306
ISR 0:15a30802e719 307 return final;
ISR 0:15a30802e719 308 }
ISR 0:15a30802e719 309
ISR 0:15a30802e719 310
ISR 0:15a30802e719 311 /**
ISR 0:15a30802e719 312 * Calculates and returns the value of the right "incremental" encoder.
ISR 0:15a30802e719 313 *
ISR 0:15a30802e719 314 * @return The value of "tics" of the right encoder since it was initialized
ISR 0:15a30802e719 315 */
ISR 0:15a30802e719 316 long int incremental_R_encoder()
ISR 0:15a30802e719 317 {
ISR 0:15a30802e719 318 short int next_R=read_R_encoder(); // Reads curent value of the encoder
ISR 0:15a30802e719 319 short int dif=next_R-prev_R; // Calculates the diference from last reading
ISR 0:15a30802e719 320
ISR 0:15a30802e719 321 if(dif>3000) { // Going back and pass zero
ISR 0:15a30802e719 322 total_R=total_R-4096+dif;
ISR 0:15a30802e719 323 }
ISR 0:15a30802e719 324 if(dif<3000&&dif>0) { // Going front
ISR 0:15a30802e719 325 total_R=total_R+dif;
ISR 0:15a30802e719 326 }
ISR 0:15a30802e719 327 if(dif<-3000) { // Going front and pass zero
ISR 0:15a30802e719 328 total_R=total_R+4096+dif;
ISR 0:15a30802e719 329 }
ISR 0:15a30802e719 330 if(dif>-3000&&dif<0) { // going back
ISR 0:15a30802e719 331 total_R=total_R+dif;
ISR 0:15a30802e719 332 }
ISR 0:15a30802e719 333 prev_R=next_R; // Sets last reading for next iteration
ISR 0:15a30802e719 334
ISR 0:15a30802e719 335 return total_R;
ISR 0:15a30802e719 336 }
ISR 0:15a30802e719 337
ISR 0:15a30802e719 338
ISR 0:15a30802e719 339 /**
ISR 0:15a30802e719 340 * Calculates and returns the value of the left "incremental" encoder.
ISR 0:15a30802e719 341 *
ISR 0:15a30802e719 342 * @return The value of "tics" of the left encoder since it was initialized
ISR 0:15a30802e719 343 */
ISR 0:15a30802e719 344 long int incremental_L_encoder()
ISR 0:15a30802e719 345 {
ISR 0:15a30802e719 346 short int next_L=read_L_encoder(); // Reads curent value of the encoder
ISR 0:15a30802e719 347 short int dif=-next_L+prev_L; // Calculates the diference from last reading
ISR 0:15a30802e719 348
ISR 0:15a30802e719 349 if(dif>3000) { // Going back and pass zero
ISR 0:15a30802e719 350 total_L=total_L-4096+dif;
ISR 0:15a30802e719 351 }
ISR 0:15a30802e719 352 if(dif<3000&&dif>0) { // Going front
ISR 0:15a30802e719 353 total_L=total_L+dif;
ISR 0:15a30802e719 354 }
ISR 0:15a30802e719 355 if(dif<-3000) { // Going front and pass zero
ISR 0:15a30802e719 356 total_L=total_L+4096+dif;
ISR 0:15a30802e719 357 }
ISR 0:15a30802e719 358 if(dif>-3000&&dif<0) { // going back
ISR 0:15a30802e719 359 total_L=total_L+dif;
ISR 0:15a30802e719 360 }
ISR 0:15a30802e719 361 prev_L=next_L; // Sets last reading for next iteration
ISR 0:15a30802e719 362
ISR 0:15a30802e719 363 return total_L;
ISR 0:15a30802e719 364 }
ISR 0:15a30802e719 365
ISR 0:15a30802e719 366
ISR 0:15a30802e719 367 /**
ISR 0:15a30802e719 368 * Calculate the value of both encoder "incremental" every 10 ms.
ISR 0:15a30802e719 369 */
ISR 0:15a30802e719 370 void timer_event() //10ms interrupt
ISR 0:15a30802e719 371 {
ISR 0:15a30802e719 372 timeout.attach(&timer_event,0.01);
ISR 0:15a30802e719 373 if(flag==0) {
ISR 0:15a30802e719 374 incremental_R_encoder();
ISR 0:15a30802e719 375 incremental_L_encoder();
ISR 0:15a30802e719 376 }
ISR 0:15a30802e719 377 Odometria();
ISR 0:15a30802e719 378
ISR 0:15a30802e719 379 return;
ISR 0:15a30802e719 380 }
ISR 0:15a30802e719 381
ISR 0:15a30802e719 382
ISR 0:15a30802e719 383 /**
ISR 0:15a30802e719 384 * Set the initial position for the "incremental" enconder and "starts" them.
ISR 0:15a30802e719 385 */
ISR 0:15a30802e719 386 void initEncoders()
ISR 0:15a30802e719 387 {
ISR 0:15a30802e719 388 prev_R=read_R_encoder();
ISR 0:15a30802e719 389 prev_L=read_L_encoder();
ISR 0:15a30802e719 390 timeout.attach(&timer_event,0.01);
ISR 0:15a30802e719 391 }
ISR 0:15a30802e719 392
ISR 0:15a30802e719 393
ISR 0:15a30802e719 394 /**
ISR 0:15a30802e719 395 * Returns to the user the value of the right "incremental" encoder.
ISR 0:15a30802e719 396 *
ISR 0:15a30802e719 397 * @return The value of "tics" of the right encoder since it was initialized
ISR 0:15a30802e719 398 */
ISR 0:15a30802e719 399 long int R_encoder()
ISR 0:15a30802e719 400 {
ISR 0:15a30802e719 401 wait(0.0001);
ISR 0:15a30802e719 402
ISR 0:15a30802e719 403 return total_R;
ISR 0:15a30802e719 404 }
ISR 0:15a30802e719 405
ISR 0:15a30802e719 406 /**
ISR 0:15a30802e719 407 * Returns to the user the value of the right "incremental" encoder.
ISR 0:15a30802e719 408 *
ISR 0:15a30802e719 409 * @return The value of "tics" of the right encoder since it was initialized
ISR 0:15a30802e719 410 */
ISR 0:15a30802e719 411 long int L_encoder()
ISR 0:15a30802e719 412 {
ISR 0:15a30802e719 413 wait(0.0001);
ISR 0:15a30802e719 414
ISR 0:15a30802e719 415 return total_L;
ISR 0:15a30802e719 416 }
ISR 0:15a30802e719 417
ISR 0:15a30802e719 418
ISR 0:15a30802e719 419 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 420 /////////////////////////////////// BATTERY ///////////////////////////////////////////
ISR 0:15a30802e719 421 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 422
ISR 0:15a30802e719 423 /**
ISR 0:15a30802e719 424 * Reads adc of the battery.
ISR 0:15a30802e719 425 *
ISR 0:15a30802e719 426 * @param addr - Address to read
ISR 0:15a30802e719 427 * @return The voltage of the batery
ISR 0:15a30802e719 428 */
ISR 0:15a30802e719 429 long int read16_mcp3424(char addr)
ISR 0:15a30802e719 430 {
ISR 0:15a30802e719 431 char data[4];
ISR 0:15a30802e719 432 i2c1.start();
ISR 0:15a30802e719 433 i2c1.read(addr,data,3);
ISR 0:15a30802e719 434 i2c1.stop();
ISR 0:15a30802e719 435
ISR 0:15a30802e719 436 return(((data[0]&127)*256)+data[1]);
ISR 0:15a30802e719 437 }
ISR 0:15a30802e719 438
ISR 0:15a30802e719 439 /**
ISR 0:15a30802e719 440 * Reads adc of the battery.
ISR 0:15a30802e719 441 *
ISR 0:15a30802e719 442 * @param n_bits - Resolution of measure
ISR 0:15a30802e719 443 * @param ch - Chose value to read, if voltage or current of solar or batery
ISR 0:15a30802e719 444 * @param gain -
ISR 0:15a30802e719 445 * @param addr - Address to write to
ISR 0:15a30802e719 446 */
ISR 0:15a30802e719 447 void write_mcp3424(int n_bits, int ch, int gain, char addr) //chanel 1-4 write -> 0xD0
ISR 0:15a30802e719 448 {
ISR 0:15a30802e719 449
ISR 0:15a30802e719 450 int chanel_end=(ch-1)<<5; //shift left
ISR 0:15a30802e719 451 char n_bits_end=0;
ISR 0:15a30802e719 452
ISR 0:15a30802e719 453 if(n_bits==12) {
ISR 0:15a30802e719 454 n_bits_end=0;
ISR 0:15a30802e719 455 } else if(n_bits==14) {
ISR 0:15a30802e719 456 n_bits_end=1;
ISR 0:15a30802e719 457 } else if(n_bits==16) {
ISR 0:15a30802e719 458 n_bits_end=2;
ISR 0:15a30802e719 459 } else {
ISR 0:15a30802e719 460 n_bits_end=3;
ISR 0:15a30802e719 461 }
ISR 0:15a30802e719 462 n_bits_end=n_bits_end<<2; //shift left
ISR 0:15a30802e719 463
ISR 0:15a30802e719 464 char data[1];
ISR 0:15a30802e719 465 data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
ISR 0:15a30802e719 466 i2c1.start();
ISR 0:15a30802e719 467 i2c1.write(addr,data,1);
ISR 0:15a30802e719 468 i2c1.stop();
ISR 0:15a30802e719 469 }
ISR 0:15a30802e719 470
ISR 0:15a30802e719 471
ISR 0:15a30802e719 472 /**
ISR 0:15a30802e719 473 * Reads adc of the battery.
ISR 0:15a30802e719 474 *
ISR 0:15a30802e719 475 * @return The voltage of the batery
ISR 0:15a30802e719 476 */
ISR 0:15a30802e719 477 float value_of_Batery()
ISR 0:15a30802e719 478 {
ISR 0:15a30802e719 479 float R1=75000.0;
ISR 0:15a30802e719 480 float R2=39200.0;
ISR 0:15a30802e719 481 float R3=178000.0;
ISR 0:15a30802e719 482 float Gain=1.0;
ISR 0:15a30802e719 483 write_mcp3424(16,3,1,0xd8);
ISR 0:15a30802e719 484 float cha3_v2=read16_mcp3424(0xd9); //read voltage
ISR 0:15a30802e719 485 float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
ISR 0:15a30802e719 486 float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
ISR 0:15a30802e719 487 Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
ISR 0:15a30802e719 488
ISR 0:15a30802e719 489 return Vin_b_v_battery;
ISR 0:15a30802e719 490 }
ISR 0:15a30802e719 491
ISR 0:15a30802e719 492
ISR 0:15a30802e719 493 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 494 ////////////////////////////////// Sonar ////////////////////////////////////////////
ISR 0:15a30802e719 495 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 496 // Commands of operation with ultrasonic module
ISR 0:15a30802e719 497
ISR 0:15a30802e719 498 // WRITE OPTION:
ISR 0:15a30802e719 499 // ENABLE DC DC CONVERTER - 0x0C;
ISR 0:15a30802e719 500 // DISABLE DC DC CONVERTER - 0x0B;
ISR 0:15a30802e719 501 // START MEASURE LEFT SENSOR - 0x0A;
ISR 0:15a30802e719 502 // START MEASURE FRONT SENSOR - 0x09;
ISR 0:15a30802e719 503 // START MEASURE RIGHT SENSOR - 0x08;
ISR 0:15a30802e719 504 // SENSORS ALWAYS MEASURE ON - 0x07;
ISR 0:15a30802e719 505 // SENSORS ALWAYS MEASURE OFF - 0x06;
ISR 0:15a30802e719 506
ISR 0:15a30802e719 507 // READ OPTION:
ISR 0:15a30802e719 508 // GET MEASURE OF LEFT SENSOR - 0x05;
ISR 0:15a30802e719 509 // GET MEASURE OF FRONT SENSOR - 0x04;
ISR 0:15a30802e719 510 // GET MEASURE OF IGHT SENSOR - 0x03;
ISR 0:15a30802e719 511 // GET STATUS SENSORS ALWAYS MEASURE - 0x02;
ISR 0:15a30802e719 512 // GET STATUS DC DC CONVERTER - 0x01;
ISR 0:15a30802e719 513
ISR 0:15a30802e719 514 void enable_dc_dc_boost()
ISR 0:15a30802e719 515 {
ISR 0:15a30802e719 516 char data[1];
ISR 0:15a30802e719 517 data[0]= 0x0C;
ISR 0:15a30802e719 518 wait_ms(1);
ISR 0:15a30802e719 519 i2c1.start();
ISR 0:15a30802e719 520 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 521 i2c1.stop();
ISR 0:15a30802e719 522 i2c1.start();
ISR 0:15a30802e719 523 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 524 i2c1.stop();
ISR 0:15a30802e719 525 }
ISR 0:15a30802e719 526
ISR 0:15a30802e719 527
ISR 0:15a30802e719 528 void disable_dc_dc_boost()
ISR 0:15a30802e719 529 {
ISR 0:15a30802e719 530 char data[1];
ISR 0:15a30802e719 531 data[0]= 0x0B;
ISR 0:15a30802e719 532 wait_ms(1);
ISR 0:15a30802e719 533 i2c1.start();
ISR 0:15a30802e719 534 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 535 i2c1.stop();
ISR 0:15a30802e719 536 }
ISR 0:15a30802e719 537
ISR 0:15a30802e719 538
ISR 0:15a30802e719 539 void start_read_left_sensor()
ISR 0:15a30802e719 540 {
ISR 0:15a30802e719 541 char data[1];
ISR 0:15a30802e719 542 data[0]= 0x0A;
ISR 0:15a30802e719 543 wait_ms(1);
ISR 0:15a30802e719 544 i2c1.start();
ISR 0:15a30802e719 545 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 546 i2c1.stop();
ISR 0:15a30802e719 547 }
ISR 0:15a30802e719 548
ISR 0:15a30802e719 549
ISR 0:15a30802e719 550 void start_read_front_sensor()
ISR 0:15a30802e719 551 {
ISR 0:15a30802e719 552 char data[1];
ISR 0:15a30802e719 553 data[0]= 0x09;
ISR 0:15a30802e719 554 wait_ms(1);
ISR 0:15a30802e719 555 i2c1.start();
ISR 0:15a30802e719 556 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 557 i2c1.stop();
ISR 0:15a30802e719 558 }
ISR 0:15a30802e719 559
ISR 0:15a30802e719 560
ISR 0:15a30802e719 561 void start_read_right_sensor()
ISR 0:15a30802e719 562 {
ISR 0:15a30802e719 563 char data[1];
ISR 0:15a30802e719 564 data[0]= 0x08;
ISR 0:15a30802e719 565 wait_ms(1);
ISR 0:15a30802e719 566 i2c1.start();
ISR 0:15a30802e719 567 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 568 i2c1.stop();
ISR 0:15a30802e719 569 }
ISR 0:15a30802e719 570
ISR 0:15a30802e719 571
ISR 0:15a30802e719 572 void measure_always_on() // left, front, right
ISR 0:15a30802e719 573 {
ISR 0:15a30802e719 574 char data[1];
ISR 0:15a30802e719 575 data[0]= 0x07;
ISR 0:15a30802e719 576 wait_ms(1);
ISR 0:15a30802e719 577 i2c1.start();
ISR 0:15a30802e719 578 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 579 i2c1.stop();
ISR 0:15a30802e719 580 }
ISR 0:15a30802e719 581
ISR 0:15a30802e719 582
ISR 0:15a30802e719 583 void measure_always_off()
ISR 0:15a30802e719 584 {
ISR 0:15a30802e719 585 char data[1];
ISR 0:15a30802e719 586 data[0]= 0x06;
ISR 0:15a30802e719 587 wait_ms(1);
ISR 0:15a30802e719 588 i2c1.start();
ISR 0:15a30802e719 589 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 590 i2c1.stop();
ISR 0:15a30802e719 591 }
ISR 0:15a30802e719 592
ISR 0:15a30802e719 593 /**
ISR 0:15a30802e719 594 * Returns left sensor value
ISR 0:15a30802e719 595 */
ISR 0:15a30802e719 596 static unsigned int get_distance_left_sensor()
ISR 0:15a30802e719 597 {
ISR 0:15a30802e719 598
ISR 0:15a30802e719 599 static char data_r[3];
ISR 0:15a30802e719 600 static unsigned int aux;
ISR 0:15a30802e719 601 flag=1;
ISR 0:15a30802e719 602
ISR 0:15a30802e719 603 data_r[0]= 0x05;
ISR 0:15a30802e719 604 wait_ms(1);
ISR 0:15a30802e719 605 i2c1.start();
ISR 0:15a30802e719 606 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 607 i2c1.stop();
ISR 0:15a30802e719 608 wait_ms(10);
ISR 0:15a30802e719 609 i2c1.start();
ISR 0:15a30802e719 610 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 611 i2c1.stop();
ISR 0:15a30802e719 612
ISR 0:15a30802e719 613 aux=(data_r[0]*256)+data_r[1];
ISR 0:15a30802e719 614 flag=0;
ISR 0:15a30802e719 615 return aux;
ISR 0:15a30802e719 616 // sensor_left=aux;
ISR 0:15a30802e719 617 // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
ISR 0:15a30802e719 618
ISR 0:15a30802e719 619 }
ISR 0:15a30802e719 620
ISR 0:15a30802e719 621
ISR 0:15a30802e719 622 /**
ISR 0:15a30802e719 623 * Returns front sensor value
ISR 0:15a30802e719 624 */
ISR 0:15a30802e719 625 static unsigned int get_distance_front_sensor()
ISR 0:15a30802e719 626 {
ISR 0:15a30802e719 627
ISR 0:15a30802e719 628 static char data_r[3];
ISR 0:15a30802e719 629 static unsigned int aux;
ISR 0:15a30802e719 630 flag=1;
ISR 0:15a30802e719 631 data_r[0]= 0x04;
ISR 0:15a30802e719 632 wait_ms(1);
ISR 0:15a30802e719 633 i2c1.start();
ISR 0:15a30802e719 634 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 635 i2c1.stop();
ISR 0:15a30802e719 636 wait_ms(10);
ISR 0:15a30802e719 637 i2c1.start();
ISR 0:15a30802e719 638 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 639 i2c1.stop();
ISR 0:15a30802e719 640
ISR 0:15a30802e719 641 aux=(data_r[0]*256)+data_r[1];
ISR 0:15a30802e719 642 flag=0;
ISR 0:15a30802e719 643 return aux;
ISR 0:15a30802e719 644 // sensor_front=aux;
ISR 0:15a30802e719 645 // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
ISR 0:15a30802e719 646
ISR 0:15a30802e719 647 }
ISR 0:15a30802e719 648
ISR 0:15a30802e719 649
ISR 0:15a30802e719 650 /**
ISR 0:15a30802e719 651 * Returns right sensor value
ISR 0:15a30802e719 652 */
ISR 0:15a30802e719 653 static unsigned int get_distance_right_sensor()
ISR 0:15a30802e719 654 {
ISR 0:15a30802e719 655
ISR 0:15a30802e719 656 static char data_r[3];
ISR 0:15a30802e719 657 static unsigned int aux;
ISR 0:15a30802e719 658 flag=1;
ISR 0:15a30802e719 659
ISR 0:15a30802e719 660 data_r[0]= 0x03;
ISR 0:15a30802e719 661 wait_ms(1);
ISR 0:15a30802e719 662 i2c1.start();
ISR 0:15a30802e719 663 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 664 i2c1.stop();
ISR 0:15a30802e719 665 wait_ms(10);
ISR 0:15a30802e719 666 i2c1.start();
ISR 0:15a30802e719 667 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 668 i2c1.stop();
ISR 0:15a30802e719 669
ISR 0:15a30802e719 670 aux=(data_r[0]*256)+data_r[1];
ISR 0:15a30802e719 671 flag=0;
ISR 0:15a30802e719 672 return aux;
ISR 0:15a30802e719 673 // sensor_right=aux;
ISR 0:15a30802e719 674 // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
ISR 0:15a30802e719 675
ISR 0:15a30802e719 676 }
ISR 0:15a30802e719 677
ISR 0:15a30802e719 678
ISR 0:15a30802e719 679 void get_status_always_measure()
ISR 0:15a30802e719 680 {
ISR 0:15a30802e719 681
ISR 0:15a30802e719 682 static char data_r[3];
ISR 0:15a30802e719 683 static unsigned int aux;
ISR 0:15a30802e719 684
ISR 0:15a30802e719 685 data_r[0]= 0x02;
ISR 0:15a30802e719 686 wait_ms(1);
ISR 0:15a30802e719 687 i2c1.start();
ISR 0:15a30802e719 688 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 689 i2c1.stop();
ISR 0:15a30802e719 690 wait_ms(10);
ISR 0:15a30802e719 691 i2c1.start();
ISR 0:15a30802e719 692 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 693 i2c1.stop();
ISR 0:15a30802e719 694
ISR 0:15a30802e719 695 aux=data_r[0];
ISR 0:15a30802e719 696 pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
ISR 0:15a30802e719 697
ISR 0:15a30802e719 698 }
ISR 0:15a30802e719 699
ISR 0:15a30802e719 700
ISR 0:15a30802e719 701 void get_status_dcdc_converter()
ISR 0:15a30802e719 702 {
ISR 0:15a30802e719 703
ISR 0:15a30802e719 704 static char data_r[3];
ISR 0:15a30802e719 705 static unsigned int aux;
ISR 0:15a30802e719 706
ISR 0:15a30802e719 707 data_r[0]= 0x01;
ISR 0:15a30802e719 708 wait_ms(1);
ISR 0:15a30802e719 709 i2c1.start();
ISR 0:15a30802e719 710 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 711 i2c1.stop();
ISR 0:15a30802e719 712 wait_ms(10);
ISR 0:15a30802e719 713 i2c1.start();
ISR 0:15a30802e719 714 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 715 i2c1.stop();
ISR 0:15a30802e719 716
ISR 0:15a30802e719 717 aux=data_r[0];
ISR 0:15a30802e719 718 pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
ISR 0:15a30802e719 719
ISR 0:15a30802e719 720 }
ISR 0:15a30802e719 721
ISR 0:15a30802e719 722
ISR 0:15a30802e719 723 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 724 ////////////////////////////////// MISC. ////////////////////////////////////////////
ISR 0:15a30802e719 725 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 726
ISR 0:15a30802e719 727
ISR 0:15a30802e719 728 /**
ISR 0:15a30802e719 729 * Initializes the necessary robot pins
ISR 0:15a30802e719 730 */
ISR 0:15a30802e719 731 void init_robot_pins()
ISR 0:15a30802e719 732 {
ISR 0:15a30802e719 733
ISR 0:15a30802e719 734 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 735 //q_pha_mot_rig=0; //Phase Motor Right
ISR 0:15a30802e719 736 //q_sleep_mot_rig=0; //Nano Sleep Motor Right
ISR 0:15a30802e719 737 //q_pha_mot_lef=0; //Phase Motor Left
ISR 0:15a30802e719 738 //q_sleep_mot_lef=0; //Nano Sleep Motor Left
ISR 0:15a30802e719 739 //q_pow_ena_i2c_p=0; //Power Enable i2c FET P
ISR 0:15a30802e719 740 //q_pow_ena_mic_p=0; //Power enable Micro FET P
ISR 0:15a30802e719 741 //q_pow_as5600_n=1; //AS5600 Power MOSFET N
ISR 0:15a30802e719 742 //q_pow_as5600_p=0; //AS5600 Power MOSFET P
ISR 0:15a30802e719 743 //q_pow_spi=0; //SPI Power MOSFET P
ISR 0:15a30802e719 744 //q_ena_mppt=0; //Enable MPPT Control
ISR 0:15a30802e719 745 //q_boost_ps=1; //Boost Power Save
ISR 0:15a30802e719 746 //q_tca9548_reset=1; //Reset TCA9548
ISR 0:15a30802e719 747
ISR 0:15a30802e719 748 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 749 q_pha_mot_rig=0; //Phase Motor Right
ISR 0:15a30802e719 750 q_sleep_mot_rig=0; //Nano Sleep Motor Right
ISR 0:15a30802e719 751 q_pha_mot_lef=0; //Phase Motor Left
ISR 0:15a30802e719 752 q_sleep_mot_lef=0; //Nano Sleep Motor Left
ISR 0:15a30802e719 753
ISR 0:15a30802e719 754 q_pow_ena_i2c_p=0; //Power Enable i2c FET P
ISR 0:15a30802e719 755 q_pow_ena_mic_p=0; //Power enable Micro FET P
ISR 0:15a30802e719 756 q_pow_as5600_p=0; //AS5600 Power MOSFET P
ISR 0:15a30802e719 757 // q_pow_spi=0; //SPI Power MOSFET P
ISR 0:15a30802e719 758 q_pow_as5600_n=1; //AS5600 Power MOSFET N
ISR 0:15a30802e719 759
ISR 0:15a30802e719 760
ISR 0:15a30802e719 761 q_ena_mppt=0; //Enable MPPT Control
ISR 0:15a30802e719 762 q_boost_ps=1; //Boost Power Save
ISR 0:15a30802e719 763 q_tca9548_reset=1; //Reset TCA9548
ISR 0:15a30802e719 764
ISR 0:15a30802e719 765 //Leds caso seja saida digital:
ISR 0:15a30802e719 766 q_led_red_fro=1; //Led Red Front (led off)
ISR 0:15a30802e719 767 q_led_gre_fro=1; //Led Green Front (led off)
ISR 0:15a30802e719 768 q_led_blu_fro=1; //Led Blue Front (led off)
ISR 0:15a30802e719 769 q_led_red_rea=1; //Led Red Rear (led off)
ISR 0:15a30802e719 770 q_led_gre_rea=1; //Led Green Rear (led off)
ISR 0:15a30802e719 771 q_led_blu_rea=1; //Led Blue Rear (led off)r
ISR 0:15a30802e719 772
ISR 0:15a30802e719 773
ISR 0:15a30802e719 774 //********************************************************************
ISR 0:15a30802e719 775 //SAIDAS DIGITAIS (pwm)
ISR 0:15a30802e719 776 //PWM Enable Motor Right
ISR 0:15a30802e719 777 pwm_mot_rig.period_us(500);
ISR 0:15a30802e719 778 pwm_mot_rig.pulsewidth_us(0);
ISR 0:15a30802e719 779
ISR 0:15a30802e719 780 //PWM Enable Motor Left
ISR 0:15a30802e719 781 pwm_mot_lef.period_us(500);
ISR 0:15a30802e719 782 pwm_mot_lef.pulsewidth_us(0);
ISR 0:15a30802e719 783
ISR 0:15a30802e719 784 //Buzzer PWM
ISR 0:15a30802e719 785 pwm_buzzer.period_us(500);
ISR 0:15a30802e719 786 pwm_buzzer.pulsewidth_us(0);
ISR 0:15a30802e719 787
ISR 0:15a30802e719 788 //LED white
ISR 0:15a30802e719 789 pwm_led_whi.period_us(500);
ISR 0:15a30802e719 790 pwm_led_whi.pulsewidth_us(0);
ISR 0:15a30802e719 791
ISR 0:15a30802e719 792 }
ISR 0:15a30802e719 793
ISR 0:15a30802e719 794
ISR 0:15a30802e719 795 /**
ISR 0:15a30802e719 796 * Initializes all the pins and all the modules necessary
ISR 0:15a30802e719 797 */
ISR 0:15a30802e719 798 void initRobot(void)
ISR 0:15a30802e719 799 {
ISR 0:15a30802e719 800 init_robot_pins();
ISR 0:15a30802e719 801 enable_dc_dc_boost();
fabiofaria 1:d1443589406e 802 //init_Infrared();
fabiofaria 1:d1443589406e 803 //initEncoders();
fabiofaria 1:d1443589406e 804
ISR 0:15a30802e719 805 enable_dc_dc_boost();
ISR 0:15a30802e719 806 wait_ms(100); //wait for read wait(>=150ms);
ISR 0:15a30802e719 807 measure_always_on();
ISR 0:15a30802e719 808 float value = value_of_Batery();
ISR 0:15a30802e719 809 pc.printf("Initialization Successful \n\r");
ISR 0:15a30802e719 810 pc.printf("Battery level: %f \n\r",value);
ISR 0:15a30802e719 811 if(value < 3.0) {
ISR 0:15a30802e719 812 pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
ISR 0:15a30802e719 813 }
ISR 0:15a30802e719 814
ISR 0:15a30802e719 815 // float level = value_of_Batery();
ISR 0:15a30802e719 816 // sendValue(int(level*100));
ISR 0:15a30802e719 817
ISR 0:15a30802e719 818 }
ISR 0:15a30802e719 819
ISR 0:15a30802e719 820
ISR 0:15a30802e719 821 ////////////////////////////////////////////////////
ISR 0:15a30802e719 822
ISR 0:15a30802e719 823 /**
ISR 0:15a30802e719 824 * Updates the position and orientation of the robot based on the data from the encoders
ISR 0:15a30802e719 825 *
ISR 0:15a30802e719 826 * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
ISR 0:15a30802e719 827 * and the distance between them is 7.4
ISR 0:15a30802e719 828 */
ISR 0:15a30802e719 829 void Odometria()
ISR 0:15a30802e719 830 {
ISR 0:15a30802e719 831 long int ticks1d=R_encoder();
ISR 0:15a30802e719 832 long int ticks1e=L_encoder();
ISR 0:15a30802e719 833
ISR 0:15a30802e719 834 long int D_ticks=ticks1d - ticks2d;
ISR 0:15a30802e719 835 long int E_ticks=ticks1e - ticks2e;
ISR 0:15a30802e719 836
ISR 0:15a30802e719 837 ticks2d=ticks1d;
ISR 0:15a30802e719 838 ticks2e=ticks1e;
ISR 0:15a30802e719 839
ISR 0:15a30802e719 840 float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
ISR 0:15a30802e719 841 float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
ISR 0:15a30802e719 842
ISR 0:15a30802e719 843 float CM=(D_cm + L_cm)/2;
ISR 0:15a30802e719 844
ISR 0:15a30802e719 845 theta +=(D_cm - L_cm)/7.18;
ISR 0:15a30802e719 846
ISR 0:15a30802e719 847 theta = atan2(sin(theta), cos(theta));
ISR 0:15a30802e719 848
ISR 0:15a30802e719 849 // meter entre 0
ISR 0:15a30802e719 850
ISR 0:15a30802e719 851 X += CM*cos(theta);
ISR 0:15a30802e719 852 Y += CM*sin(theta);
ISR 0:15a30802e719 853
ISR 0:15a30802e719 854 }