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