Yo Fleyt / ISR_Mini-explorer

ouais ok

Dependents:   roboticLab_withclass_3_July

Fork of ISR_Mini-explorer by ISR UC

robot.h@2:92ca9a6a1d4e, 2017-07-11 (annotated)

Committer:
Ludwigfr
Date:
Tue Jul 11 16:54:58 2017 +0000
Revision:
2:92ca9a6a1d4e
Parent:
1:b81277897a5b
Child:
3:b996cbe5dabd
11/07; 16:54

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 #include "nRF24L01P.h"
ISR 0:15a30802e719 21
ISR 0:15a30802e719 22 void Odometria();
Ludwigfr 1:b81277897a5b 23 void OdometriaKalman(float* R_cm, float* L_cm);
ISR 0:15a30802e719 24
ISR 0:15a30802e719 25 // classes adicionais
ISR 0:15a30802e719 26 nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
ISR 0:15a30802e719 27 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
ISR 0:15a30802e719 28 Timeout timeout;
ISR 0:15a30802e719 29
ISR 0:15a30802e719 30 Serial pc(PTE0,PTE1);
ISR 0:15a30802e719 31 I2C i2c(PTC9,PTC8);
ISR 0:15a30802e719 32 I2C i2c1(PTC11,PTC10);
ISR 0:15a30802e719 33
ISR 0:15a30802e719 34 // Variables needed by the lib
ISR 0:15a30802e719 35 unsigned int ProxiValue=0;
ISR 0:15a30802e719 36 short int prev_R=0;
ISR 0:15a30802e719 37 short int prev_L=0;
ISR 0:15a30802e719 38 long int total_R=0;
ISR 0:15a30802e719 39 long int total_L=0;
ISR 0:15a30802e719 40 long int ticks2d=0;
ISR 0:15a30802e719 41 long int ticks2e=0;
ISR 0:15a30802e719 42 float X=20;
ISR 0:15a30802e719 43 float Y=20;
ISR 0:15a30802e719 44 float AtractX = 0;
ISR 0:15a30802e719 45 float AtractY = 0;
ISR 0:15a30802e719 46 float theta=0;
ISR 0:15a30802e719 47 int sensor_left=0;
ISR 0:15a30802e719 48 int sensor_front=0;
ISR 0:15a30802e719 49 int sensor_right=0;
ISR 0:15a30802e719 50 short int flag=0;
ISR 0:15a30802e719 51 int IRobot=0;
ISR 0:15a30802e719 52 int JRobot=0;
ISR 0:15a30802e719 53
Ludwigfr 2:92ca9a6a1d4e 54 float D_cm=0;
Ludwigfr 2:92ca9a6a1d4e 55 float L_cm=0;
Ludwigfr 2:92ca9a6a1d4e 56
ISR 0:15a30802e719 57 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 58 DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right
ISR 0:15a30802e719 59 DigitalOut q_sleep_mot_rig (PTE3,0); //Nano Sleep Motor Right
ISR 0:15a30802e719 60 DigitalOut q_pha_mot_lef (PTA17,0); //Phase Motor Left
ISR 0:15a30802e719 61 DigitalOut q_sleep_mot_lef (PTB11,0); //Nano Sleep Motor Left
ISR 0:15a30802e719 62 DigitalOut q_pow_ena_i2c_p (PTE2,0); //Power Enable i2c FET P (0- enable 1-disable)
ISR 0:15a30802e719 63 DigitalOut q_pow_ena_mic_p (PTA14,0); //Power enable Micro FET P (0- enable 1-disable)
ISR 0:15a30802e719 64 DigitalOut q_pow_as5600_n (PTC6,1); //AS5600 Power MOSFET N (1- enable 0-disable)
ISR 0:15a30802e719 65 DigitalOut q_pow_as5600_p (PTC5,0); //AS5600 Power MOSFET P (0- enable 1-disable)
ISR 0:15a30802e719 66 DigitalOut q_pow_spi (PTC4,0); //SPI Power MOSFET P (0- enable 1-disable)
ISR 0:15a30802e719 67 DigitalOut q_ena_mppt (PTC0,0); //Enable MPPT Control (0- enable 1-disable)
ISR 0:15a30802e719 68 DigitalOut q_boost_ps (PTC7,1); //Boost Power Save (1- enable 0-disable)
ISR 0:15a30802e719 69 DigitalOut q_tca9548_reset (PTC3,1); //Reset TCA9548 (1- enable 0-disable)
ISR 0:15a30802e719 70 DigitalOut power_36khz (PTD0,0); //Power enable pic12f - 36khz (0- enable 1-disable)
ISR 0:15a30802e719 71
ISR 0:15a30802e719 72
ISR 0:15a30802e719 73 // ********************************************************************
ISR 0:15a30802e719 74 // ********************************************************************
ISR 0:15a30802e719 75 //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
ISR 0:15a30802e719 76 //ENTRADAS DIGITAIS (normal input)
ISR 0:15a30802e719 77 DigitalIn i_enc_dir_rig (PTB8); //Encoder Right Direction
ISR 0:15a30802e719 78 DigitalIn i_enc_dir_lef (PTB9); //Encoder Left Direction
ISR 0:15a30802e719 79 DigitalIn i_micro_sd_det (PTC16); //MICRO SD Card Detect
ISR 0:15a30802e719 80 DigitalIn i_mppt_fail (PTE5); //Fail MPPT Signal
ISR 0:15a30802e719 81 DigitalIn i_usb_volt (PTB10); //USB Voltage detect
ISR 0:15a30802e719 82 DigitalIn i_sup_cap_est (PTB19); //Supercap State Charger
ISR 0:15a30802e719 83 DigitalIn i_li_ion_est (PTB18); //Li-ion State Charger
ISR 0:15a30802e719 84
ISR 0:15a30802e719 85
ISR 0:15a30802e719 86 // ********************************************************************
ISR 0:15a30802e719 87 //ENTRADAS DIGITAIS (external interrupt)
ISR 0:15a30802e719 88 InterruptIn i_int_mpu9250 (PTA15); //Interrupt MPU9250
ISR 0:15a30802e719 89 InterruptIn i_int_isl29125 (PTA16); //Interrupt ISL29125 Color S.
ISR 0:15a30802e719 90 InterruptIn i_mic_f_l (PTD7); //Interrupt Comp Micro F L
ISR 0:15a30802e719 91 InterruptIn i_mic_f_r (PTD6); //Interrupt Comp Micro F R
ISR 0:15a30802e719 92 InterruptIn i_mic_r_c (PTD5); //Interrupt Comp Micro R C
ISR 0:15a30802e719 93
ISR 0:15a30802e719 94
ISR 0:15a30802e719 95 // ********************************************************************
ISR 0:15a30802e719 96 //ENTRADAS ANALOGICAS
ISR 0:15a30802e719 97 AnalogIn a_enc_rig (PTC2); //Encoder Left Output_AS_MR
ISR 0:15a30802e719 98 AnalogIn a_enc_lef (PTC1); //Encoder Right Output_AS_ML
ISR 0:15a30802e719 99 AnalogIn a_mic_f_l (PTB0); //Analog microphone F L
ISR 0:15a30802e719 100 AnalogIn a_mic_f_r (PTB1); //Analog microphone F R
ISR 0:15a30802e719 101 AnalogIn a_mic_r_c (PTB2); //Analog microphone R C
ISR 0:15a30802e719 102 AnalogIn a_temp_bat (PTB3); //Temperature Battery
ISR 0:15a30802e719 103
ISR 0:15a30802e719 104
ISR 0:15a30802e719 105 // ********************************************************************
ISR 0:15a30802e719 106
ISR 0:15a30802e719 107 //PWM OR DIGITAL OUTPUT NORMAL
ISR 0:15a30802e719 108 //DigitalOut q_led_whi (PTE29); //Led white pwm
ISR 0:15a30802e719 109 DigitalOut q_led_red_fro (PTA4); //Led Red Front
ISR 0:15a30802e719 110 DigitalOut q_led_gre_fro (PTA5); //Led Green Front
ISR 0:15a30802e719 111 DigitalOut q_led_blu_fro (PTA12); //Led Blue Front
ISR 0:15a30802e719 112 DigitalOut q_led_red_rea (PTD4); //Led Red Rear
ISR 0:15a30802e719 113 DigitalOut q_led_gre_rea (PTA1); //Led Green Rear
ISR 0:15a30802e719 114 DigitalOut q_led_blu_rea (PTA2); //Led Blue Rear
ISR 0:15a30802e719 115
ISR 0:15a30802e719 116
ISR 0:15a30802e719 117 //SAIDAS DIGITAIS (pwm)
ISR 0:15a30802e719 118 PwmOut pwm_mot_rig (PTE20); //PWM Enable Motor Right
ISR 0:15a30802e719 119 PwmOut pwm_mot_lef (PTE31); //PWM Enable Motor Left
ISR 0:15a30802e719 120 PwmOut pwm_buzzer (PTE21); //Buzzer PWM
ISR 0:15a30802e719 121 PwmOut pwm_led_whi (PTE29); //Led white pwm
ISR 0:15a30802e719 122
ISR 0:15a30802e719 123 // ********************************************************************
ISR 0:15a30802e719 124 //SAIDAS ANALOGICAS
ISR 0:15a30802e719 125 AnalogOut dac_comp_mic (PTE30); //Dac_Comparator MIC
ISR 0:15a30802e719 126
ISR 0:15a30802e719 127
ISR 0:15a30802e719 128 /* Powers up all the VCNL4020. */
ISR 0:15a30802e719 129 void init_Infrared()
ISR 0:15a30802e719 130 {
ISR 0:15a30802e719 131 VCNL40x0_Device.SetCurrent (20); // Set current to 200mA
ISR 0:15a30802e719 132 }
ISR 0:15a30802e719 133
ISR 0:15a30802e719 134 /**
ISR 0:15a30802e719 135 * Selects the wich infrared to comunicate.
ISR 0:15a30802e719 136 *
ISR 0:15a30802e719 137 * @param ch - Infrared to read (1..5)
ISR 0:15a30802e719 138 */
ISR 0:15a30802e719 139 void tca9548_select_ch(char ch)
ISR 0:15a30802e719 140 {
ISR 0:15a30802e719 141 char ch_f[1];
ISR 0:15a30802e719 142 char addr=0xE0;
ISR 0:15a30802e719 143
ISR 0:15a30802e719 144 if(ch==0)
ISR 0:15a30802e719 145 ch_f[0]=1;
ISR 0:15a30802e719 146
ISR 0:15a30802e719 147 if(ch>=1)
ISR 0:15a30802e719 148 ch_f[0]=1<<ch;
ISR 0:15a30802e719 149
ISR 0:15a30802e719 150 i2c.start();
ISR 0:15a30802e719 151 i2c.write(addr,ch_f,1);
ISR 0:15a30802e719 152 i2c.stop();
ISR 0:15a30802e719 153 }
ISR 0:15a30802e719 154
ISR 0:15a30802e719 155
ISR 0:15a30802e719 156 /**
ISR 0:15a30802e719 157 * Get ADC value of the chosen infrared.
ISR 0:15a30802e719 158 *
ISR 0:15a30802e719 159 * @param ch - Infrared to read (1..5)
ISR 0:15a30802e719 160 *
ISR 0:15a30802e719 161 * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
ISR 0:15a30802e719 162 */
ISR 0:15a30802e719 163 long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
ISR 0:15a30802e719 164 {
ISR 0:15a30802e719 165 tca9548_select_ch(ch);
ISR 0:15a30802e719 166 VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue); // read prox value on demand
ISR 0:15a30802e719 167
ISR 0:15a30802e719 168 return ProxiValue;
ISR 0:15a30802e719 169 }
ISR 0:15a30802e719 170
ISR 0:15a30802e719 171 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 172 /////////////////////////////////// MOTOR ///////////////////////////////////////////
ISR 0:15a30802e719 173 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 174
ISR 0:15a30802e719 175 // 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 176 // consultar pag 39 e 95
ISR 0:15a30802e719 177
ISR 0:15a30802e719 178 /**
ISR 0:15a30802e719 179 * Sets speed and direction of the left motor.
ISR 0:15a30802e719 180 *
ISR 0:15a30802e719 181 * @param Dir - Direction of movement, 0 for back, or 1 for fron
ISR 0:15a30802e719 182 * @param Speed - Percentage of speed of the motor (1..100)
ISR 0:15a30802e719 183 *
ISR 0:15a30802e719 184 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
ISR 0:15a30802e719 185 * at different speeds and see if it makes a straigth line
ISR 0:15a30802e719 186 */
ISR 0:15a30802e719 187 void leftMotor(short int Dir,short int Speed)
ISR 0:15a30802e719 188 {
ISR 0:15a30802e719 189 float Duty;
ISR 0:15a30802e719 190
ISR 0:15a30802e719 191 if(Dir==1) {
ISR 0:15a30802e719 192 q_pha_mot_lef=0; //Andar em frente
ISR 0:15a30802e719 193 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 194 Speed=1000;
ISR 0:15a30802e719 195 if(Speed>0) {
ISR 0:15a30802e719 196 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 197 q_sleep_mot_lef=1; //Nano Sleep Motor Left
ISR 0:15a30802e719 198 pwm_mot_lef.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 199 } else {
ISR 0:15a30802e719 200 q_sleep_mot_lef=0;
ISR 0:15a30802e719 201 }
ISR 0:15a30802e719 202 }
ISR 0:15a30802e719 203 if(Dir==0) {
ISR 0:15a30802e719 204 q_pha_mot_lef=1; //Andar para tras
ISR 0:15a30802e719 205
ISR 0:15a30802e719 206 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 207 Speed=1000;
ISR 0:15a30802e719 208 if(Speed>0) {
ISR 0:15a30802e719 209 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 210 q_sleep_mot_lef=1; //Nano Sleep Motor Left
ISR 0:15a30802e719 211 pwm_mot_lef.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 212 } else {
ISR 0:15a30802e719 213 q_sleep_mot_lef=0;
ISR 0:15a30802e719 214 }
ISR 0:15a30802e719 215 }
ISR 0:15a30802e719 216 }
ISR 0:15a30802e719 217
ISR 0:15a30802e719 218
ISR 0:15a30802e719 219 /**
ISR 0:15a30802e719 220 * Sets speed and direction of the right motor.
ISR 0:15a30802e719 221 *
ISR 0:15a30802e719 222 * @param Dir - Direction of movement, 0 for back, or 1 for fron
ISR 0:15a30802e719 223 * @param Speed - Percentage of speed of the motor (1..100)
ISR 0:15a30802e719 224 *
ISR 0:15a30802e719 225 * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
ISR 0:15a30802e719 226 * at different speeds and see if it makes a straigth line
ISR 0:15a30802e719 227 */
ISR 0:15a30802e719 228 void rightMotor(short int Dir,short int Speed)
ISR 0:15a30802e719 229 {
ISR 0:15a30802e719 230 float Duty;
ISR 0:15a30802e719 231
ISR 0:15a30802e719 232 if(Dir==1) {
ISR 0:15a30802e719 233 q_pha_mot_rig=0; //Andar em frente
ISR 0:15a30802e719 234
ISR 0:15a30802e719 235 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 236 Speed=1000;
ISR 0:15a30802e719 237 if(Speed>0) {
ISR 0:15a30802e719 238 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 239 q_sleep_mot_rig=1; //Nano Sleep Motor Right
ISR 0:15a30802e719 240 pwm_mot_rig.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 241 } else {
ISR 0:15a30802e719 242 q_sleep_mot_rig=0;
ISR 0:15a30802e719 243 }
ISR 0:15a30802e719 244 }
ISR 0:15a30802e719 245 if(Dir==0) {
ISR 0:15a30802e719 246 q_pha_mot_rig=1; //Andar para tras
ISR 0:15a30802e719 247
ISR 0:15a30802e719 248
ISR 0:15a30802e719 249 if(Speed>1000) //limite de segurança
ISR 0:15a30802e719 250 Speed=1000;
ISR 0:15a30802e719 251 if(Speed>0) {
ISR 0:15a30802e719 252 Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
ISR 0:15a30802e719 253 q_sleep_mot_rig=1; //Nano Sleep Motor Right
ISR 0:15a30802e719 254 pwm_mot_rig.pulsewidth_us(Duty*5);
ISR 0:15a30802e719 255 } else {
ISR 0:15a30802e719 256 q_sleep_mot_rig=0;
ISR 0:15a30802e719 257 }
ISR 0:15a30802e719 258 }
ISR 0:15a30802e719 259 }
ISR 0:15a30802e719 260
ISR 0:15a30802e719 261
ISR 0:15a30802e719 262 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 263 /////////////////////////////////// ENCODER ///////////////////////////////////////////
ISR 0:15a30802e719 264 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 265
ISR 0:15a30802e719 266 /**
ISR 0:15a30802e719 267 * Reads Position of left magnetic encoder.
ISR 0:15a30802e719 268 *
ISR 0:15a30802e719 269 * @return The absolute position of the left wheel encoder (0..4095)
ISR 0:15a30802e719 270 */
ISR 0:15a30802e719 271 long int read_L_encoder()
ISR 0:15a30802e719 272 {
ISR 0:15a30802e719 273
ISR 0:15a30802e719 274 char data_r_2[5];
ISR 0:15a30802e719 275
ISR 0:15a30802e719 276 i2c.start();
ISR 0:15a30802e719 277 i2c.write(0x6C);
ISR 0:15a30802e719 278 i2c.write(0x0C);
ISR 0:15a30802e719 279 i2c.read(0x6D,data_r_2,4,0);
ISR 0:15a30802e719 280 i2c.stop();
ISR 0:15a30802e719 281
ISR 0:15a30802e719 282 short int val1=data_r_2[0];
ISR 0:15a30802e719 283 short int val2=data_r_2[1];
ISR 0:15a30802e719 284 val1=(val1&0xf)*256;
ISR 0:15a30802e719 285 long int final=(val2+val1);
ISR 0:15a30802e719 286
ISR 0:15a30802e719 287 return final;
ISR 0:15a30802e719 288 }
ISR 0:15a30802e719 289
ISR 0:15a30802e719 290
ISR 0:15a30802e719 291 /**
ISR 0:15a30802e719 292 * Reads Position of right magnetic encoder.
ISR 0:15a30802e719 293 *
ISR 0:15a30802e719 294 * @return The absolute position of the right wheel encoder (0..4095)
ISR 0:15a30802e719 295 */
ISR 0:15a30802e719 296 long int read_R_encoder()
ISR 0:15a30802e719 297 {
ISR 0:15a30802e719 298
ISR 0:15a30802e719 299 char data_r_2[5];
ISR 0:15a30802e719 300
ISR 0:15a30802e719 301 i2c1.start();
ISR 0:15a30802e719 302 i2c1.write(0x6C);
ISR 0:15a30802e719 303 i2c1.write(0x0C);
ISR 0:15a30802e719 304 i2c1.read(0x6D,data_r_2,4,0);
ISR 0:15a30802e719 305 i2c1.stop();
ISR 0:15a30802e719 306
ISR 0:15a30802e719 307 short int val1=data_r_2[0];
ISR 0:15a30802e719 308 short int val2=data_r_2[1];
ISR 0:15a30802e719 309 val1=(val1&0xf)*256;
ISR 0:15a30802e719 310 long int final=(val2+val1);
ISR 0:15a30802e719 311
ISR 0:15a30802e719 312 return final;
ISR 0:15a30802e719 313 }
ISR 0:15a30802e719 314
ISR 0:15a30802e719 315
ISR 0:15a30802e719 316 /**
ISR 0:15a30802e719 317 * Calculates and returns the value of the right "incremental" encoder.
ISR 0:15a30802e719 318 *
ISR 0:15a30802e719 319 * @return The value of "tics" of the right encoder since it was initialized
ISR 0:15a30802e719 320 */
ISR 0:15a30802e719 321 long int incremental_R_encoder()
ISR 0:15a30802e719 322 {
ISR 0:15a30802e719 323 short int next_R=read_R_encoder(); // Reads curent value of the encoder
ISR 0:15a30802e719 324 short int dif=next_R-prev_R; // Calculates the diference from last reading
ISR 0:15a30802e719 325
ISR 0:15a30802e719 326 if(dif>3000) { // Going back and pass zero
ISR 0:15a30802e719 327 total_R=total_R-4096+dif;
ISR 0:15a30802e719 328 }
ISR 0:15a30802e719 329 if(dif<3000&&dif>0) { // Going front
ISR 0:15a30802e719 330 total_R=total_R+dif;
ISR 0:15a30802e719 331 }
ISR 0:15a30802e719 332 if(dif<-3000) { // Going front and pass zero
ISR 0:15a30802e719 333 total_R=total_R+4096+dif;
ISR 0:15a30802e719 334 }
ISR 0:15a30802e719 335 if(dif>-3000&&dif<0) { // going back
ISR 0:15a30802e719 336 total_R=total_R+dif;
ISR 0:15a30802e719 337 }
ISR 0:15a30802e719 338 prev_R=next_R; // Sets last reading for next iteration
ISR 0:15a30802e719 339
ISR 0:15a30802e719 340 return total_R;
ISR 0:15a30802e719 341 }
ISR 0:15a30802e719 342
ISR 0:15a30802e719 343
ISR 0:15a30802e719 344 /**
ISR 0:15a30802e719 345 * Calculates and returns the value of the left "incremental" encoder.
ISR 0:15a30802e719 346 *
ISR 0:15a30802e719 347 * @return The value of "tics" of the left encoder since it was initialized
ISR 0:15a30802e719 348 */
ISR 0:15a30802e719 349 long int incremental_L_encoder()
ISR 0:15a30802e719 350 {
ISR 0:15a30802e719 351 short int next_L=read_L_encoder(); // Reads curent value of the encoder
ISR 0:15a30802e719 352 short int dif=-next_L+prev_L; // Calculates the diference from last reading
ISR 0:15a30802e719 353
ISR 0:15a30802e719 354 if(dif>3000) { // Going back and pass zero
ISR 0:15a30802e719 355 total_L=total_L-4096+dif;
ISR 0:15a30802e719 356 }
ISR 0:15a30802e719 357 if(dif<3000&&dif>0) { // Going front
ISR 0:15a30802e719 358 total_L=total_L+dif;
ISR 0:15a30802e719 359 }
ISR 0:15a30802e719 360 if(dif<-3000) { // Going front and pass zero
ISR 0:15a30802e719 361 total_L=total_L+4096+dif;
ISR 0:15a30802e719 362 }
ISR 0:15a30802e719 363 if(dif>-3000&&dif<0) { // going back
ISR 0:15a30802e719 364 total_L=total_L+dif;
ISR 0:15a30802e719 365 }
ISR 0:15a30802e719 366 prev_L=next_L; // Sets last reading for next iteration
ISR 0:15a30802e719 367
ISR 0:15a30802e719 368 return total_L;
ISR 0:15a30802e719 369 }
ISR 0:15a30802e719 370
ISR 0:15a30802e719 371
ISR 0:15a30802e719 372 /**
ISR 0:15a30802e719 373 * Calculate the value of both encoder "incremental" every 10 ms.
ISR 0:15a30802e719 374 */
ISR 0:15a30802e719 375 void timer_event() //10ms interrupt
ISR 0:15a30802e719 376 {
ISR 0:15a30802e719 377 timeout.attach(&timer_event,0.01);
ISR 0:15a30802e719 378 if(flag==0) {
ISR 0:15a30802e719 379 incremental_R_encoder();
ISR 0:15a30802e719 380 incremental_L_encoder();
ISR 0:15a30802e719 381 }
ISR 0:15a30802e719 382 Odometria();
ISR 0:15a30802e719 383
ISR 0:15a30802e719 384 return;
ISR 0:15a30802e719 385 }
ISR 0:15a30802e719 386
ISR 0:15a30802e719 387
ISR 0:15a30802e719 388 /**
ISR 0:15a30802e719 389 * Set the initial position for the "incremental" enconder and "starts" them.
ISR 0:15a30802e719 390 */
ISR 0:15a30802e719 391 void initEncoders()
ISR 0:15a30802e719 392 {
ISR 0:15a30802e719 393 prev_R=read_R_encoder();
ISR 0:15a30802e719 394 prev_L=read_L_encoder();
ISR 0:15a30802e719 395 timeout.attach(&timer_event,0.01);
ISR 0:15a30802e719 396 }
ISR 0:15a30802e719 397
ISR 0:15a30802e719 398
ISR 0:15a30802e719 399 /**
ISR 0:15a30802e719 400 * Returns to the user the value of the right "incremental" encoder.
ISR 0:15a30802e719 401 *
ISR 0:15a30802e719 402 * @return The value of "tics" of the right encoder since it was initialized
ISR 0:15a30802e719 403 */
ISR 0:15a30802e719 404 long int R_encoder()
ISR 0:15a30802e719 405 {
ISR 0:15a30802e719 406 wait(0.0001);
ISR 0:15a30802e719 407
ISR 0:15a30802e719 408 return total_R;
ISR 0:15a30802e719 409 }
ISR 0:15a30802e719 410
ISR 0:15a30802e719 411 /**
ISR 0:15a30802e719 412 * Returns to the user the value of the right "incremental" encoder.
ISR 0:15a30802e719 413 *
ISR 0:15a30802e719 414 * @return The value of "tics" of the right encoder since it was initialized
ISR 0:15a30802e719 415 */
ISR 0:15a30802e719 416 long int L_encoder()
ISR 0:15a30802e719 417 {
ISR 0:15a30802e719 418 wait(0.0001);
ISR 0:15a30802e719 419
ISR 0:15a30802e719 420 return total_L;
ISR 0:15a30802e719 421 }
ISR 0:15a30802e719 422
ISR 0:15a30802e719 423
ISR 0:15a30802e719 424 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 425 /////////////////////////////////// BATTERY ///////////////////////////////////////////
ISR 0:15a30802e719 426 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 427
ISR 0:15a30802e719 428 /**
ISR 0:15a30802e719 429 * Reads adc of the battery.
ISR 0:15a30802e719 430 *
ISR 0:15a30802e719 431 * @param addr - Address to read
ISR 0:15a30802e719 432 * @return The voltage of the batery
ISR 0:15a30802e719 433 */
ISR 0:15a30802e719 434 long int read16_mcp3424(char addr)
ISR 0:15a30802e719 435 {
ISR 0:15a30802e719 436 char data[4];
ISR 0:15a30802e719 437 i2c1.start();
ISR 0:15a30802e719 438 i2c1.read(addr,data,3);
ISR 0:15a30802e719 439 i2c1.stop();
ISR 0:15a30802e719 440
ISR 0:15a30802e719 441 return(((data[0]&127)*256)+data[1]);
ISR 0:15a30802e719 442 }
ISR 0:15a30802e719 443
ISR 0:15a30802e719 444 /**
ISR 0:15a30802e719 445 * Reads adc of the battery.
ISR 0:15a30802e719 446 *
ISR 0:15a30802e719 447 * @param n_bits - Resolution of measure
ISR 0:15a30802e719 448 * @param ch - Chose value to read, if voltage or current of solar or batery
ISR 0:15a30802e719 449 * @param gain -
ISR 0:15a30802e719 450 * @param addr - Address to write to
ISR 0:15a30802e719 451 */
ISR 0:15a30802e719 452 void write_mcp3424(int n_bits, int ch, int gain, char addr) //chanel 1-4 write -> 0xD0
ISR 0:15a30802e719 453 {
ISR 0:15a30802e719 454
ISR 0:15a30802e719 455 int chanel_end=(ch-1)<<5; //shift left
ISR 0:15a30802e719 456 char n_bits_end=0;
ISR 0:15a30802e719 457
ISR 0:15a30802e719 458 if(n_bits==12) {
ISR 0:15a30802e719 459 n_bits_end=0;
ISR 0:15a30802e719 460 } else if(n_bits==14) {
ISR 0:15a30802e719 461 n_bits_end=1;
ISR 0:15a30802e719 462 } else if(n_bits==16) {
ISR 0:15a30802e719 463 n_bits_end=2;
ISR 0:15a30802e719 464 } else {
ISR 0:15a30802e719 465 n_bits_end=3;
ISR 0:15a30802e719 466 }
ISR 0:15a30802e719 467 n_bits_end=n_bits_end<<2; //shift left
ISR 0:15a30802e719 468
ISR 0:15a30802e719 469 char data[1];
ISR 0:15a30802e719 470 data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
ISR 0:15a30802e719 471 i2c1.start();
ISR 0:15a30802e719 472 i2c1.write(addr,data,1);
ISR 0:15a30802e719 473 i2c1.stop();
ISR 0:15a30802e719 474 }
ISR 0:15a30802e719 475
ISR 0:15a30802e719 476
ISR 0:15a30802e719 477 /**
ISR 0:15a30802e719 478 * Reads adc of the battery.
ISR 0:15a30802e719 479 *
ISR 0:15a30802e719 480 * @return The voltage of the batery
ISR 0:15a30802e719 481 */
ISR 0:15a30802e719 482 float value_of_Batery()
ISR 0:15a30802e719 483 {
ISR 0:15a30802e719 484 float R1=75000.0;
ISR 0:15a30802e719 485 float R2=39200.0;
ISR 0:15a30802e719 486 float R3=178000.0;
ISR 0:15a30802e719 487 float Gain=1.0;
ISR 0:15a30802e719 488 write_mcp3424(16,3,1,0xd8);
ISR 0:15a30802e719 489 float cha3_v2=read16_mcp3424(0xd9); //read voltage
ISR 0:15a30802e719 490 float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
ISR 0:15a30802e719 491 float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
ISR 0:15a30802e719 492 Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
ISR 0:15a30802e719 493
ISR 0:15a30802e719 494 return Vin_b_v_battery;
ISR 0:15a30802e719 495 }
ISR 0:15a30802e719 496
ISR 0:15a30802e719 497 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 498 /////////////////////////////// RF COMUNICATION ///////////////////////////////////////
ISR 0:15a30802e719 499 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 500
ISR 0:15a30802e719 501
ISR 0:15a30802e719 502 /**
ISR 0:15a30802e719 503 * Initializes the NRF24 module for comunication.
ISR 0:15a30802e719 504 *
ISR 0:15a30802e719 505 * Note: if the module is broken or badly connected this init will cause the code to stop,
ISR 0:15a30802e719 506 * if all these messages don't appear thats the case
ISR 0:15a30802e719 507 */
ISR 0:15a30802e719 508 void config_init_nrf()
ISR 0:15a30802e719 509 {
ISR 0:15a30802e719 510 my_nrf24l01p.powerUp(); // powers module
ISR 0:15a30802e719 511 my_nrf24l01p.setRfFrequency (2400); // channel 0 (2400-0 ... 2516-116)
ISR 0:15a30802e719 512 my_nrf24l01p.setTransferSize(10); // number of bytes to be transfer
ISR 0:15a30802e719 513 my_nrf24l01p.setCrcWidth(8);
ISR 0:15a30802e719 514 my_nrf24l01p.enableAutoAcknowledge(NRF24L01P_PIPE_P0); // pipe where data transfer occurs (0..6)
ISR 0:15a30802e719 515 pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() );
ISR 0:15a30802e719 516 pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
ISR 0:15a30802e719 517 pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
ISR 0:15a30802e719 518 pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
ISR 0:15a30802e719 519 pc.printf( "nRF24L01+ CrC Width : %d CrC\r\n", my_nrf24l01p.getCrcWidth() );
ISR 0:15a30802e719 520 pc.printf( "nRF24L01+ TransferSize : %d Paket Size\r\n", my_nrf24l01p.getTransferSize () );
ISR 0:15a30802e719 521 my_nrf24l01p.setReceiveMode();
ISR 0:15a30802e719 522 my_nrf24l01p.enable();
ISR 0:15a30802e719 523 pc.printf( "Setup complete, Starting While loop\r\n");
ISR 0:15a30802e719 524 }
ISR 0:15a30802e719 525
ISR 0:15a30802e719 526
ISR 0:15a30802e719 527 /**
ISR 0:15a30802e719 528 * Receives a number from the Arduino.
ISR 0:15a30802e719 529 *
ISR 0:15a30802e719 530 * @return The value send by the arduino
ISR 0:15a30802e719 531 */
ISR 0:15a30802e719 532 double receiveValue(void)
ISR 0:15a30802e719 533 {
ISR 0:15a30802e719 534 char temp[4];
ISR 0:15a30802e719 535 double Val;
ISR 0:15a30802e719 536 bool ok=0;
ISR 0:15a30802e719 537 my_nrf24l01p.setTransferSize(4);
ISR 0:15a30802e719 538 my_nrf24l01p.setReceiveMode();
ISR 0:15a30802e719 539 my_nrf24l01p.enable();
ISR 0:15a30802e719 540 do {
ISR 0:15a30802e719 541 if(my_nrf24l01p.readable(NRF24L01P_PIPE_P0)) {
ISR 0:15a30802e719 542 ok = my_nrf24l01p.read( NRF24L01P_PIPE_P0,temp, 4);
ISR 0:15a30802e719 543 }
ISR 0:15a30802e719 544 } while(ok==0);
ISR 0:15a30802e719 545
ISR 0:15a30802e719 546 //transformation of temp to convert to original value
ISR 0:15a30802e719 547 if(temp[0]==0) // if first elemente is 0 then its negative
ISR 0:15a30802e719 548 Val = double(-(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255));
ISR 0:15a30802e719 549 else // else its positive
ISR 0:15a30802e719 550 Val = double(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255);
ISR 0:15a30802e719 551
ISR 0:15a30802e719 552 return Val;
ISR 0:15a30802e719 553 }
ISR 0:15a30802e719 554
ISR 0:15a30802e719 555
ISR 0:15a30802e719 556 /**
ISR 0:15a30802e719 557 * Sends a number to the Arduino
ISR 0:15a30802e719 558 *
ISR 0:15a30802e719 559 * @param Value - number to be sent to the Arduino
ISR 0:15a30802e719 560 */
ISR 0:15a30802e719 561 void sendValue(long int Value)
ISR 0:15a30802e719 562 {
ISR 0:15a30802e719 563 bool ok=0; // comunication sucess, o if failed 1 if sucessfull
ISR 0:15a30802e719 564 // double math=Value/65025; // temporary variable save results
ISR 0:15a30802e719 565 int zero=1; // 1 byte, ( - ) if 0 ( + ) if 1
ISR 0:15a30802e719 566 int one=0; // 2 byte (0..255), multiplied by 1
ISR 0:15a30802e719 567 int two=0; // 3 byte (0..255), multiplied by 255
ISR 0:15a30802e719 568 int three=0; // 4 byte (0..255), multiplied by 255*255
ISR 0:15a30802e719 569
ISR 0:15a30802e719 570 //transformation of Value to send correctly through pipe
ISR 0:15a30802e719 571 if (Value<0) {
ISR 0:15a30802e719 572 zero=0;
ISR 0:15a30802e719 573 Value=abs(Value);
ISR 0:15a30802e719 574 }
ISR 0:15a30802e719 575 // Value=abs(Value);
ISR 0:15a30802e719 576
ISR 0:15a30802e719 577 double math=Value/65025; // temporary variable save results
ISR 0:15a30802e719 578
ISR 0:15a30802e719 579 if(math<1) {
ISR 0:15a30802e719 580 math=Value/255;
ISR 0:15a30802e719 581 if(math<1) {
ISR 0:15a30802e719 582 two=0;
ISR 0:15a30802e719 583 one=Value;
ISR 0:15a30802e719 584 } else {
ISR 0:15a30802e719 585 two=(int)math;
ISR 0:15a30802e719 586 one=Value % 255;
ISR 0:15a30802e719 587 }
ISR 0:15a30802e719 588 } else {
ISR 0:15a30802e719 589 three=(int)math;
ISR 0:15a30802e719 590 math=Value/255;
ISR 0:15a30802e719 591 if(math<1) {
ISR 0:15a30802e719 592 two=0;
ISR 0:15a30802e719 593 one=Value;
ISR 0:15a30802e719 594 } else {
ISR 0:15a30802e719 595 two=(int)math;
ISR 0:15a30802e719 596 one=Value % 255;
ISR 0:15a30802e719 597 }
ISR 0:15a30802e719 598 }
ISR 0:15a30802e719 599 char temp[4]= {(int)zero,(int)one,(int)two,(int)three};
ISR 0:15a30802e719 600
ISR 0:15a30802e719 601 // Apagar depois de testar mais vezes
ISR 0:15a30802e719 602 // pc.printf("1 inidice...%i...\r", temp[0]);
ISR 0:15a30802e719 603 // pc.printf("2 inidice...%i...\r", temp[1]);
ISR 0:15a30802e719 604 // pc.printf("3 inidice...%i...\r", temp[2]);
ISR 0:15a30802e719 605 // pc.printf("4 inidice...%i...\r", temp[3]);
ISR 0:15a30802e719 606
ISR 0:15a30802e719 607 my_nrf24l01p.setTransferSize(4);
ISR 0:15a30802e719 608 my_nrf24l01p.setTransmitMode();
ISR 0:15a30802e719 609 my_nrf24l01p.enable();
ISR 0:15a30802e719 610 do {
ISR 0:15a30802e719 611 ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,temp, 4);
ISR 0:15a30802e719 612 if(ok==1)
ISR 0:15a30802e719 613 pc.printf("Done.....%i...\r", Value);
ISR 0:15a30802e719 614 else {
ISR 0:15a30802e719 615 pc.printf("Failed\r");
ISR 0:15a30802e719 616 wait(1);
ISR 0:15a30802e719 617 }
ISR 0:15a30802e719 618 } while(ok==0);
ISR 0:15a30802e719 619 }
ISR 0:15a30802e719 620
ISR 0:15a30802e719 621 /**
ISR 0:15a30802e719 622 * Sends matrix to arduino.
ISR 0:15a30802e719 623 *
ISR 0:15a30802e719 624 * @param matrix - Matrix of numbers to send [0..255]
ISR 0:15a30802e719 625 * @param row - Number of rows
ISR 0:15a30802e719 626 * @param column - Number of columns
ISR 0:15a30802e719 627 */
ISR 0:15a30802e719 628 void sendMatrix(int (*matrix)[18],int row , int column)
ISR 0:15a30802e719 629 {
ISR 0:15a30802e719 630 short ok=0;
ISR 0:15a30802e719 631 short int i =0;
ISR 0:15a30802e719 632 short int j=0;
ISR 0:15a30802e719 633 short int byte=0;
ISR 0:15a30802e719 634 int members=column*row;
ISR 0:15a30802e719 635 char message[32]= {0};
ISR 0:15a30802e719 636 pc.printf("J ...%d... \r",members);
ISR 0:15a30802e719 637
ISR 0:15a30802e719 638 my_nrf24l01p.setTransferSize(32);
ISR 0:15a30802e719 639 my_nrf24l01p.setTransmitMode();
ISR 0:15a30802e719 640 my_nrf24l01p.enable();
ISR 0:15a30802e719 641
ISR 0:15a30802e719 642 do {
ISR 0:15a30802e719 643 int* point = matrix[j];
ISR 0:15a30802e719 644
ISR 0:15a30802e719 645 do {
ISR 0:15a30802e719 646 message[byte]= point[i];
ISR 0:15a30802e719 647 if (byte==31 || (i+1)*(j+1)==members) {
ISR 0:15a30802e719 648
ISR 0:15a30802e719 649 do {
ISR 0:15a30802e719 650 ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,message, 32);
ISR 0:15a30802e719 651 if(ok==0)
ISR 0:15a30802e719 652 wait(1);
ISR 0:15a30802e719 653
ISR 0:15a30802e719 654 } while(ok==0);
ISR 0:15a30802e719 655
ISR 0:15a30802e719 656 byte=-1;
ISR 0:15a30802e719 657 }
ISR 0:15a30802e719 658
ISR 0:15a30802e719 659 byte++;
ISR 0:15a30802e719 660 i++;
ISR 0:15a30802e719 661
ISR 0:15a30802e719 662 } while(i<column);
ISR 0:15a30802e719 663
ISR 0:15a30802e719 664 i=0;
ISR 0:15a30802e719 665 j++;
ISR 0:15a30802e719 666 } while(j<row);
ISR 0:15a30802e719 667
ISR 0:15a30802e719 668 }
ISR 0:15a30802e719 669
ISR 0:15a30802e719 670 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 671 ////////////////////////////////// Sonar ////////////////////////////////////////////
ISR 0:15a30802e719 672 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 673 // Commands of operation with ultrasonic module
ISR 0:15a30802e719 674
ISR 0:15a30802e719 675 // WRITE OPTION:
ISR 0:15a30802e719 676 // ENABLE DC DC CONVERTER - 0x0C;
ISR 0:15a30802e719 677 // DISABLE DC DC CONVERTER - 0x0B;
ISR 0:15a30802e719 678 // START MEASURE LEFT SENSOR - 0x0A;
ISR 0:15a30802e719 679 // START MEASURE FRONT SENSOR - 0x09;
ISR 0:15a30802e719 680 // START MEASURE RIGHT SENSOR - 0x08;
ISR 0:15a30802e719 681 // SENSORS ALWAYS MEASURE ON - 0x07;
ISR 0:15a30802e719 682 // SENSORS ALWAYS MEASURE OFF - 0x06;
ISR 0:15a30802e719 683
ISR 0:15a30802e719 684 // READ OPTION:
ISR 0:15a30802e719 685 // GET MEASURE OF LEFT SENSOR - 0x05;
ISR 0:15a30802e719 686 // GET MEASURE OF FRONT SENSOR - 0x04;
ISR 0:15a30802e719 687 // GET MEASURE OF IGHT SENSOR - 0x03;
ISR 0:15a30802e719 688 // GET STATUS SENSORS ALWAYS MEASURE - 0x02;
ISR 0:15a30802e719 689 // GET STATUS DC DC CONVERTER - 0x01;
ISR 0:15a30802e719 690
ISR 0:15a30802e719 691 void enable_dc_dc_boost()
ISR 0:15a30802e719 692 {
ISR 0:15a30802e719 693 char data[1];
ISR 0:15a30802e719 694 data[0]= 0x0C;
ISR 0:15a30802e719 695 wait_ms(1);
ISR 0:15a30802e719 696 i2c1.start();
ISR 0:15a30802e719 697 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 698 i2c1.stop();
ISR 0:15a30802e719 699 i2c1.start();
ISR 0:15a30802e719 700 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 701 i2c1.stop();
ISR 0:15a30802e719 702 }
ISR 0:15a30802e719 703
ISR 0:15a30802e719 704
ISR 0:15a30802e719 705 void disable_dc_dc_boost()
ISR 0:15a30802e719 706 {
ISR 0:15a30802e719 707 char data[1];
ISR 0:15a30802e719 708 data[0]= 0x0B;
ISR 0:15a30802e719 709 wait_ms(1);
ISR 0:15a30802e719 710 i2c1.start();
ISR 0:15a30802e719 711 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 712 i2c1.stop();
ISR 0:15a30802e719 713 }
ISR 0:15a30802e719 714
ISR 0:15a30802e719 715
ISR 0:15a30802e719 716 void start_read_left_sensor()
ISR 0:15a30802e719 717 {
ISR 0:15a30802e719 718 char data[1];
ISR 0:15a30802e719 719 data[0]= 0x0A;
ISR 0:15a30802e719 720 wait_ms(1);
ISR 0:15a30802e719 721 i2c1.start();
ISR 0:15a30802e719 722 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 723 i2c1.stop();
ISR 0:15a30802e719 724 }
ISR 0:15a30802e719 725
ISR 0:15a30802e719 726
ISR 0:15a30802e719 727 void start_read_front_sensor()
ISR 0:15a30802e719 728 {
ISR 0:15a30802e719 729 char data[1];
ISR 0:15a30802e719 730 data[0]= 0x09;
ISR 0:15a30802e719 731 wait_ms(1);
ISR 0:15a30802e719 732 i2c1.start();
ISR 0:15a30802e719 733 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 734 i2c1.stop();
ISR 0:15a30802e719 735 }
ISR 0:15a30802e719 736
ISR 0:15a30802e719 737
ISR 0:15a30802e719 738 void start_read_right_sensor()
ISR 0:15a30802e719 739 {
ISR 0:15a30802e719 740 char data[1];
ISR 0:15a30802e719 741 data[0]= 0x08;
ISR 0:15a30802e719 742 wait_ms(1);
ISR 0:15a30802e719 743 i2c1.start();
ISR 0:15a30802e719 744 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 745 i2c1.stop();
ISR 0:15a30802e719 746 }
ISR 0:15a30802e719 747
ISR 0:15a30802e719 748
ISR 0:15a30802e719 749 void measure_always_on() // left, front, right
ISR 0:15a30802e719 750 {
ISR 0:15a30802e719 751 char data[1];
ISR 0:15a30802e719 752 data[0]= 0x07;
ISR 0:15a30802e719 753 wait_ms(1);
ISR 0:15a30802e719 754 i2c1.start();
ISR 0:15a30802e719 755 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 756 i2c1.stop();
ISR 0:15a30802e719 757 }
ISR 0:15a30802e719 758
ISR 0:15a30802e719 759
ISR 0:15a30802e719 760 void measure_always_off()
ISR 0:15a30802e719 761 {
ISR 0:15a30802e719 762 char data[1];
ISR 0:15a30802e719 763 data[0]= 0x06;
ISR 0:15a30802e719 764 wait_ms(1);
ISR 0:15a30802e719 765 i2c1.start();
ISR 0:15a30802e719 766 i2c1.write(0x30,data,1);
ISR 0:15a30802e719 767 i2c1.stop();
ISR 0:15a30802e719 768 }
ISR 0:15a30802e719 769
ISR 0:15a30802e719 770 /**
ISR 0:15a30802e719 771 * Returns left sensor value
ISR 0:15a30802e719 772 */
ISR 0:15a30802e719 773 static unsigned int get_distance_left_sensor()
ISR 0:15a30802e719 774 {
ISR 0:15a30802e719 775
ISR 0:15a30802e719 776 static char data_r[3];
ISR 0:15a30802e719 777 static unsigned int aux;
ISR 0:15a30802e719 778 flag=1;
ISR 0:15a30802e719 779
ISR 0:15a30802e719 780 data_r[0]= 0x05;
ISR 0:15a30802e719 781 wait_ms(1);
ISR 0:15a30802e719 782 i2c1.start();
ISR 0:15a30802e719 783 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 784 i2c1.stop();
ISR 0:15a30802e719 785 wait_ms(10);
ISR 0:15a30802e719 786 i2c1.start();
ISR 0:15a30802e719 787 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 788 i2c1.stop();
ISR 0:15a30802e719 789
ISR 0:15a30802e719 790 aux=(data_r[0]*256)+data_r[1];
ISR 0:15a30802e719 791 flag=0;
ISR 0:15a30802e719 792 return aux;
ISR 0:15a30802e719 793 // sensor_left=aux;
ISR 0:15a30802e719 794 // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
ISR 0:15a30802e719 795
ISR 0:15a30802e719 796 }
ISR 0:15a30802e719 797
ISR 0:15a30802e719 798
ISR 0:15a30802e719 799 /**
ISR 0:15a30802e719 800 * Returns front sensor value
ISR 0:15a30802e719 801 */
ISR 0:15a30802e719 802 static unsigned int get_distance_front_sensor()
ISR 0:15a30802e719 803 {
ISR 0:15a30802e719 804
ISR 0:15a30802e719 805 static char data_r[3];
ISR 0:15a30802e719 806 static unsigned int aux;
ISR 0:15a30802e719 807 flag=1;
ISR 0:15a30802e719 808 data_r[0]= 0x04;
ISR 0:15a30802e719 809 wait_ms(1);
ISR 0:15a30802e719 810 i2c1.start();
ISR 0:15a30802e719 811 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 812 i2c1.stop();
ISR 0:15a30802e719 813 wait_ms(10);
ISR 0:15a30802e719 814 i2c1.start();
ISR 0:15a30802e719 815 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 816 i2c1.stop();
ISR 0:15a30802e719 817
ISR 0:15a30802e719 818 aux=(data_r[0]*256)+data_r[1];
ISR 0:15a30802e719 819 flag=0;
ISR 0:15a30802e719 820 return aux;
ISR 0:15a30802e719 821 // sensor_front=aux;
ISR 0:15a30802e719 822 // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
ISR 0:15a30802e719 823
ISR 0:15a30802e719 824 }
ISR 0:15a30802e719 825
ISR 0:15a30802e719 826
ISR 0:15a30802e719 827 /**
ISR 0:15a30802e719 828 * Returns right sensor value
ISR 0:15a30802e719 829 */
ISR 0:15a30802e719 830 static unsigned int get_distance_right_sensor()
ISR 0:15a30802e719 831 {
ISR 0:15a30802e719 832
ISR 0:15a30802e719 833 static char data_r[3];
ISR 0:15a30802e719 834 static unsigned int aux;
ISR 0:15a30802e719 835 flag=1;
ISR 0:15a30802e719 836
ISR 0:15a30802e719 837 data_r[0]= 0x03;
ISR 0:15a30802e719 838 wait_ms(1);
ISR 0:15a30802e719 839 i2c1.start();
ISR 0:15a30802e719 840 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 841 i2c1.stop();
ISR 0:15a30802e719 842 wait_ms(10);
ISR 0:15a30802e719 843 i2c1.start();
ISR 0:15a30802e719 844 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 845 i2c1.stop();
ISR 0:15a30802e719 846
ISR 0:15a30802e719 847 aux=(data_r[0]*256)+data_r[1];
ISR 0:15a30802e719 848 flag=0;
ISR 0:15a30802e719 849 return aux;
ISR 0:15a30802e719 850 // sensor_right=aux;
ISR 0:15a30802e719 851 // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
ISR 0:15a30802e719 852
ISR 0:15a30802e719 853 }
ISR 0:15a30802e719 854
ISR 0:15a30802e719 855
ISR 0:15a30802e719 856 void get_status_always_measure()
ISR 0:15a30802e719 857 {
ISR 0:15a30802e719 858
ISR 0:15a30802e719 859 static char data_r[3];
ISR 0:15a30802e719 860 static unsigned int aux;
ISR 0:15a30802e719 861
ISR 0:15a30802e719 862 data_r[0]= 0x02;
ISR 0:15a30802e719 863 wait_ms(1);
ISR 0:15a30802e719 864 i2c1.start();
ISR 0:15a30802e719 865 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 866 i2c1.stop();
ISR 0:15a30802e719 867 wait_ms(10);
ISR 0:15a30802e719 868 i2c1.start();
ISR 0:15a30802e719 869 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 870 i2c1.stop();
ISR 0:15a30802e719 871
ISR 0:15a30802e719 872 aux=data_r[0];
ISR 0:15a30802e719 873 pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
ISR 0:15a30802e719 874
ISR 0:15a30802e719 875 }
ISR 0:15a30802e719 876
ISR 0:15a30802e719 877
ISR 0:15a30802e719 878 void get_status_dcdc_converter()
ISR 0:15a30802e719 879 {
ISR 0:15a30802e719 880
ISR 0:15a30802e719 881 static char data_r[3];
ISR 0:15a30802e719 882 static unsigned int aux;
ISR 0:15a30802e719 883
ISR 0:15a30802e719 884 data_r[0]= 0x01;
ISR 0:15a30802e719 885 wait_ms(1);
ISR 0:15a30802e719 886 i2c1.start();
ISR 0:15a30802e719 887 i2c1.write(0x30,data_r,1);
ISR 0:15a30802e719 888 i2c1.stop();
ISR 0:15a30802e719 889 wait_ms(10);
ISR 0:15a30802e719 890 i2c1.start();
ISR 0:15a30802e719 891 i2c1.read(0x31,data_r,2,0);
ISR 0:15a30802e719 892 i2c1.stop();
ISR 0:15a30802e719 893
ISR 0:15a30802e719 894 aux=data_r[0];
ISR 0:15a30802e719 895 pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
ISR 0:15a30802e719 896
ISR 0:15a30802e719 897 }
ISR 0:15a30802e719 898
ISR 0:15a30802e719 899
ISR 0:15a30802e719 900 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 901 ////////////////////////////////// MISC. ////////////////////////////////////////////
ISR 0:15a30802e719 902 ///////////////////////////////////////////////////////////////////////////////////////////////
ISR 0:15a30802e719 903
ISR 0:15a30802e719 904
ISR 0:15a30802e719 905 /**
ISR 0:15a30802e719 906 * Initializes the necessary robot pins
ISR 0:15a30802e719 907 */
ISR 0:15a30802e719 908 void init_robot_pins()
ISR 0:15a30802e719 909 {
ISR 0:15a30802e719 910
ISR 0:15a30802e719 911 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 912 //q_pha_mot_rig=0; //Phase Motor Right
ISR 0:15a30802e719 913 //q_sleep_mot_rig=0; //Nano Sleep Motor Right
ISR 0:15a30802e719 914 //q_pha_mot_lef=0; //Phase Motor Left
ISR 0:15a30802e719 915 //q_sleep_mot_lef=0; //Nano Sleep Motor Left
ISR 0:15a30802e719 916 //q_pow_ena_i2c_p=0; //Power Enable i2c FET P
ISR 0:15a30802e719 917 //q_pow_ena_mic_p=0; //Power enable Micro FET P
ISR 0:15a30802e719 918 //q_pow_as5600_n=1; //AS5600 Power MOSFET N
ISR 0:15a30802e719 919 //q_pow_as5600_p=0; //AS5600 Power MOSFET P
ISR 0:15a30802e719 920 //q_pow_spi=0; //SPI Power MOSFET P
ISR 0:15a30802e719 921 //q_ena_mppt=0; //Enable MPPT Control
ISR 0:15a30802e719 922 //q_boost_ps=1; //Boost Power Save
ISR 0:15a30802e719 923 //q_tca9548_reset=1; //Reset TCA9548
ISR 0:15a30802e719 924
ISR 0:15a30802e719 925 //SAIDAS DIGITAIS (normal)
ISR 0:15a30802e719 926 q_pha_mot_rig=0; //Phase Motor Right
ISR 0:15a30802e719 927 q_sleep_mot_rig=0; //Nano Sleep Motor Right
ISR 0:15a30802e719 928 q_pha_mot_lef=0; //Phase Motor Left
ISR 0:15a30802e719 929 q_sleep_mot_lef=0; //Nano Sleep Motor Left
ISR 0:15a30802e719 930
ISR 0:15a30802e719 931 q_pow_ena_i2c_p=0; //Power Enable i2c FET P
ISR 0:15a30802e719 932 q_pow_ena_mic_p=0; //Power enable Micro FET P
ISR 0:15a30802e719 933 q_pow_as5600_p=0; //AS5600 Power MOSFET P
ISR 0:15a30802e719 934 // q_pow_spi=0; //SPI Power MOSFET P
ISR 0:15a30802e719 935 q_pow_as5600_n=1; //AS5600 Power MOSFET N
ISR 0:15a30802e719 936
ISR 0:15a30802e719 937
ISR 0:15a30802e719 938 q_ena_mppt=0; //Enable MPPT Control
ISR 0:15a30802e719 939 q_boost_ps=1; //Boost Power Save
ISR 0:15a30802e719 940 q_tca9548_reset=1; //Reset TCA9548
ISR 0:15a30802e719 941
ISR 0:15a30802e719 942 //Leds caso seja saida digital:
ISR 0:15a30802e719 943 q_led_red_fro=1; //Led Red Front (led off)
ISR 0:15a30802e719 944 q_led_gre_fro=1; //Led Green Front (led off)
ISR 0:15a30802e719 945 q_led_blu_fro=1; //Led Blue Front (led off)
ISR 0:15a30802e719 946 q_led_red_rea=1; //Led Red Rear (led off)
ISR 0:15a30802e719 947 q_led_gre_rea=1; //Led Green Rear (led off)
ISR 0:15a30802e719 948 q_led_blu_rea=1; //Led Blue Rear (led off)r
ISR 0:15a30802e719 949
ISR 0:15a30802e719 950
ISR 0:15a30802e719 951 //********************************************************************
ISR 0:15a30802e719 952 //SAIDAS DIGITAIS (pwm)
ISR 0:15a30802e719 953 //PWM Enable Motor Right
ISR 0:15a30802e719 954 pwm_mot_rig.period_us(500);
ISR 0:15a30802e719 955 pwm_mot_rig.pulsewidth_us(0);
ISR 0:15a30802e719 956
ISR 0:15a30802e719 957 //PWM Enable Motor Left
ISR 0:15a30802e719 958 pwm_mot_lef.period_us(500);
ISR 0:15a30802e719 959 pwm_mot_lef.pulsewidth_us(0);
ISR 0:15a30802e719 960
ISR 0:15a30802e719 961 //Buzzer PWM
ISR 0:15a30802e719 962 pwm_buzzer.period_us(500);
ISR 0:15a30802e719 963 pwm_buzzer.pulsewidth_us(0);
ISR 0:15a30802e719 964
ISR 0:15a30802e719 965 //LED white
ISR 0:15a30802e719 966 pwm_led_whi.period_us(500);
ISR 0:15a30802e719 967 pwm_led_whi.pulsewidth_us(0);
ISR 0:15a30802e719 968
ISR 0:15a30802e719 969 }
ISR 0:15a30802e719 970
ISR 0:15a30802e719 971
ISR 0:15a30802e719 972 /**
ISR 0:15a30802e719 973 * Initializes all the pins and all the modules necessary
ISR 0:15a30802e719 974 */
ISR 0:15a30802e719 975 void initRobot(void)
ISR 0:15a30802e719 976 {
ISR 0:15a30802e719 977 init_robot_pins();
ISR 0:15a30802e719 978 enable_dc_dc_boost();
ISR 0:15a30802e719 979 init_Infrared();
ISR 0:15a30802e719 980 initEncoders();
ISR 0:15a30802e719 981 config_init_nrf();
ISR 0:15a30802e719 982 enable_dc_dc_boost();
ISR 0:15a30802e719 983 wait_ms(100); //wait for read wait(>=150ms);
ISR 0:15a30802e719 984 measure_always_on();
ISR 0:15a30802e719 985 float value = value_of_Batery();
ISR 0:15a30802e719 986 pc.printf("Initialization Successful \n\r");
ISR 0:15a30802e719 987 pc.printf("Battery level: %f \n\r",value);
ISR 0:15a30802e719 988 if(value < 3.0) {
ISR 0:15a30802e719 989 pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
ISR 0:15a30802e719 990 }
ISR 0:15a30802e719 991
ISR 0:15a30802e719 992 // float level = value_of_Batery();
ISR 0:15a30802e719 993 // sendValue(int(level*100));
ISR 0:15a30802e719 994
ISR 0:15a30802e719 995 }
ISR 0:15a30802e719 996
ISR 0:15a30802e719 997
ISR 0:15a30802e719 998 ////////////////////////////////////////////////////
ISR 0:15a30802e719 999
ISR 0:15a30802e719 1000 /**
ISR 0:15a30802e719 1001 * Updates the position and orientation of the robot based on the data from the encoders
ISR 0:15a30802e719 1002 *
ISR 0:15a30802e719 1003 * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
ISR 0:15a30802e719 1004 * and the distance between them is 7.4
ISR 0:15a30802e719 1005 */
ISR 0:15a30802e719 1006 void Odometria()
ISR 0:15a30802e719 1007 {
ISR 0:15a30802e719 1008 long int ticks1d=R_encoder();
ISR 0:15a30802e719 1009 long int ticks1e=L_encoder();
Ludwigfr 2:92ca9a6a1d4e 1010 //pc.printf("\r\nticks1d:%f",ticks1d);
Ludwigfr 2:92ca9a6a1d4e 1011 //pc.printf("\r\nticks1e:%f",ticks1d);
ISR 0:15a30802e719 1012 long int D_ticks=ticks1d - ticks2d;
ISR 0:15a30802e719 1013 long int E_ticks=ticks1e - ticks2e;
Ludwigfr 2:92ca9a6a1d4e 1014 //pc.printf("\r\nD_ticks:%f",D_ticks);
Ludwigfr 2:92ca9a6a1d4e 1015 //pc.printf("\r\nE_ticks:%f",E_ticks);
ISR 0:15a30802e719 1016 ticks2d=ticks1d;
ISR 0:15a30802e719 1017 ticks2e=ticks1e;
ISR 0:15a30802e719 1018
Ludwigfr 2:92ca9a6a1d4e 1019 D_cm= (float)D_ticks*((3.25*3.1415)/4096);
Ludwigfr 2:92ca9a6a1d4e 1020 L_cm= (float)E_ticks*((3.25*3.1415)/4096);
Ludwigfr 2:92ca9a6a1d4e 1021 //pc.printf("\r\nD_cm:%f",D_cm);
Ludwigfr 2:92ca9a6a1d4e 1022 //pc.printf("\r\nL_cm:%f",L_cm);
ISR 0:15a30802e719 1023 float CM=(D_cm + L_cm)/2;
Ludwigfr 2:92ca9a6a1d4e 1024 //pc.printf("\r\nCM:%f",CM);
ISR 0:15a30802e719 1025 theta +=(D_cm - L_cm)/7.18;
ISR 0:15a30802e719 1026 theta = atan2(sin(theta), cos(theta));
ISR 0:15a30802e719 1027
ISR 0:15a30802e719 1028 // meter entre 0
ISR 0:15a30802e719 1029 X += CM*cos(theta);
ISR 0:15a30802e719 1030 Y += CM*sin(theta);
ISR 0:15a30802e719 1031
Ludwigfr 1:b81277897a5b 1032 }
Ludwigfr 1:b81277897a5b 1033
Ludwigfr 1:b81277897a5b 1034 void OdometriaKalman(float* R_cm, float* L_cm)
Ludwigfr 1:b81277897a5b 1035 {
Ludwigfr 1:b81277897a5b 1036 long int ticks1d=R_encoder();
Ludwigfr 1:b81277897a5b 1037 long int ticks1e=L_encoder();
Ludwigfr 1:b81277897a5b 1038
Ludwigfr 1:b81277897a5b 1039 long int D_ticks=ticks1d - ticks2d;
Ludwigfr 1:b81277897a5b 1040 long int E_ticks=ticks1e - ticks2e;
Ludwigfr 1:b81277897a5b 1041
Ludwigfr 1:b81277897a5b 1042 ticks2d=ticks1d;
Ludwigfr 1:b81277897a5b 1043 ticks2e=ticks1e;
Ludwigfr 1:b81277897a5b 1044
Ludwigfr 2:92ca9a6a1d4e 1045 float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
Ludwigfr 2:92ca9a6a1d4e 1046 float E_cm= (float)E_ticks*((3.25*3.1415)/4096);
Ludwigfr 2:92ca9a6a1d4e 1047
Ludwigfr 2:92ca9a6a1d4e 1048 float CM=(D_cm + E_cm)/2;
Ludwigfr 2:92ca9a6a1d4e 1049
Ludwigfr 2:92ca9a6a1d4e 1050 theta +=(D_cm - E_cm)/7.18;
Ludwigfr 2:92ca9a6a1d4e 1051
Ludwigfr 2:92ca9a6a1d4e 1052 theta = atan2(sin(theta), cos(theta));
Ludwigfr 1:b81277897a5b 1053
Ludwigfr 2:92ca9a6a1d4e 1054 // meter entre 0
Ludwigfr 2:92ca9a6a1d4e 1055
Ludwigfr 2:92ca9a6a1d4e 1056 X += CM*cos(theta);
Ludwigfr 2:92ca9a6a1d4e 1057 Y += CM*sin(theta);
Ludwigfr 2:92ca9a6a1d4e 1058
Ludwigfr 2:92ca9a6a1d4e 1059 *R_cm= D_cm;
Ludwigfr 2:92ca9a6a1d4e 1060 *L_cm= E_cm;
ISR 0:15a30802e719 1061 }