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