Fabio Faria
Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Fork of
ISR_Mini-explorer
by 
ISR UC
robot.h@1:8569ac717e68, 2018-02-20 (annotated)
- Committer:
- ISR
- Date:
- Tue Feb 20 13:37:47 2018 +0000
- Revision:
- 1:8569ac717e68
- Parent:
- 0:15a30802e719
- Child:
- 2:0435d1171673
Encoders working inside thread.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ISR | 0:15a30802e719 | 1 | /** @file */ |
| ISR | 0:15a30802e719 | 2 | |
| ISR | 0:15a30802e719 | 3 | #include <math.h> |
| ISR | 0:15a30802e719 | 4 | #include <string.h> |
| ISR | 0:15a30802e719 | 5 | #include "VCNL40x0.h" |
| ISR | 1:8569ac717e68 | 6 | #include <RF24.h> |
| ISR | 1:8569ac717e68 | 7 | #include "Encoder.h" |
| ISR | 1:8569ac717e68 | 8 | |
| ISR | 1:8569ac717e68 | 9 | I2C i2c(PTC9,PTC8); |
| ISR | 1:8569ac717e68 | 10 | I2C i2c1(PTC11,PTC10); |
| ISR | 1:8569ac717e68 | 11 | |
| ISR | 1:8569ac717e68 | 12 | |
| ISR | 1:8569ac717e68 | 13 | float X=20; |
| ISR | 1:8569ac717e68 | 14 | float Y=20; |
| ISR | 1:8569ac717e68 | 15 | |
| ISR | 1:8569ac717e68 | 16 | Mutex mutex_i2c0, mutex_i2c1; |
| ISR | 1:8569ac717e68 | 17 | Encoder esquerdo(&i2c, &mutex_i2c0, 0); |
| ISR | 1:8569ac717e68 | 18 | Encoder direito(&i2c1, &mutex_i2c1, 1); |
| ISR | 1:8569ac717e68 | 19 | |
| ISR | 1:8569ac717e68 | 20 | Thread thread; |
| ISR | 1:8569ac717e68 | 21 | |
| ISR | 1:8569ac717e68 | 22 | |
| ISR | 1:8569ac717e68 | 23 | |
| ISR | 1:8569ac717e68 | 24 | void odometry_thread() |
| ISR | 1:8569ac717e68 | 25 | { |
| ISR | 1:8569ac717e68 | 26 | float theta=0; |
| ISR | 1:8569ac717e68 | 27 | long int ticks2d=0; |
| ISR | 1:8569ac717e68 | 28 | long int ticks2e=0; |
| ISR | 1:8569ac717e68 | 29 | |
| ISR | 1:8569ac717e68 | 30 | |
| ISR | 0:15a30802e719 | 31 | |
| ISR | 1:8569ac717e68 | 32 | while (true) { |
| ISR | 1:8569ac717e68 | 33 | esquerdo.incremental(); |
| ISR | 1:8569ac717e68 | 34 | direito.incremental(); |
| ISR | 1:8569ac717e68 | 35 | //------------------------------------------- |
| ISR | 1:8569ac717e68 | 36 | long int ticks1d=direito.readIncrementalValue(); |
| ISR | 1:8569ac717e68 | 37 | long int ticks1e=esquerdo.readIncrementalValue(); |
| ISR | 1:8569ac717e68 | 38 | |
| ISR | 1:8569ac717e68 | 39 | long int D_ticks=ticks1d - ticks2d; |
| ISR | 1:8569ac717e68 | 40 | long int E_ticks=ticks1e - ticks2e; |
| ISR | 1:8569ac717e68 | 41 | |
| ISR | 1:8569ac717e68 | 42 | ticks2d=ticks1d; |
| ISR | 1:8569ac717e68 | 43 | ticks2e=ticks1e; |
| ISR | 1:8569ac717e68 | 44 | |
| ISR | 1:8569ac717e68 | 45 | float D_cm= (float)D_ticks*((3.25*3.1415)/4096); |
| ISR | 1:8569ac717e68 | 46 | float L_cm= (float)E_ticks*((3.25*3.1415)/4096); |
| ISR | 1:8569ac717e68 | 47 | |
| ISR | 1:8569ac717e68 | 48 | float CM=(D_cm + L_cm)/2; |
| ISR | 1:8569ac717e68 | 49 | |
| ISR | 1:8569ac717e68 | 50 | theta +=(D_cm - L_cm)/7.18; |
| ISR | 1:8569ac717e68 | 51 | |
| ISR | 1:8569ac717e68 | 52 | theta = atan2(sin(theta), cos(theta)); |
| ISR | 1:8569ac717e68 | 53 | |
| ISR | 1:8569ac717e68 | 54 | // meter entre 0 |
| ISR | 1:8569ac717e68 | 55 | |
| ISR | 1:8569ac717e68 | 56 | X += CM*cos(theta); |
| ISR | 1:8569ac717e68 | 57 | Y += CM*sin(theta); |
| ISR | 1:8569ac717e68 | 58 | //------------------------------------------- |
| ISR | 1:8569ac717e68 | 59 | |
| ISR | 1:8569ac717e68 | 60 | Thread::wait(10); |
| ISR | 1:8569ac717e68 | 61 | } |
| ISR | 1:8569ac717e68 | 62 | } |
| ISR | 1:8569ac717e68 | 63 | |
| ISR | 0:15a30802e719 | 64 | |
| ISR | 0:15a30802e719 | 65 | // classes adicionais |
| ISR | 1:8569ac717e68 | 66 | RF24 radio(PTD2, PTD3, PTD1, PTC12 ,PTC13); |
| ISR | 0:15a30802e719 | 67 | VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS); |
| ISR | 1:8569ac717e68 | 68 | |
| ISR | 1:8569ac717e68 | 69 | |
| ISR | 0:15a30802e719 | 70 | Timeout timeout; |
| ISR | 0:15a30802e719 | 71 | |
| ISR | 0:15a30802e719 | 72 | Serial pc(PTE0,PTE1); |
| ISR | 1:8569ac717e68 | 73 | |
| ISR | 0:15a30802e719 | 74 | |
| ISR | 0:15a30802e719 | 75 | // Variables needed by the lib |
| ISR | 0:15a30802e719 | 76 | unsigned int ProxiValue=0; |
| ISR | 0:15a30802e719 | 77 | |
| ISR | 0:15a30802e719 | 78 | //SAIDAS DIGITAIS (normal) |
| ISR | 0:15a30802e719 | 79 | DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right |
| ISR | 0:15a30802e719 | 80 | DigitalOut q_sleep_mot_rig (PTE3,0); //Nano Sleep Motor Right |
| ISR | 0:15a30802e719 | 81 | DigitalOut q_pha_mot_lef (PTA17,0); //Phase Motor Left |
| ISR | 0:15a30802e719 | 82 | DigitalOut q_sleep_mot_lef (PTB11,0); //Nano Sleep Motor Left |
| ISR | 0:15a30802e719 | 83 | DigitalOut q_pow_ena_i2c_p (PTE2,0); //Power Enable i2c FET P (0- enable 1-disable) |
| ISR | 0:15a30802e719 | 84 | DigitalOut q_pow_ena_mic_p (PTA14,0); //Power enable Micro FET P (0- enable 1-disable) |
| ISR | 0:15a30802e719 | 85 | DigitalOut q_pow_as5600_n (PTC6,1); //AS5600 Power MOSFET N (1- enable 0-disable) |
| ISR | 0:15a30802e719 | 86 | DigitalOut q_pow_as5600_p (PTC5,0); //AS5600 Power MOSFET P (0- enable 1-disable) |
| ISR | 0:15a30802e719 | 87 | DigitalOut q_pow_spi (PTC4,0); //SPI Power MOSFET P (0- enable 1-disable) |
| ISR | 0:15a30802e719 | 88 | DigitalOut q_ena_mppt (PTC0,0); //Enable MPPT Control (0- enable 1-disable) |
| ISR | 0:15a30802e719 | 89 | DigitalOut q_boost_ps (PTC7,1); //Boost Power Save (1- enable 0-disable) |
| ISR | 0:15a30802e719 | 90 | DigitalOut q_tca9548_reset (PTC3,1); //Reset TCA9548 (1- enable 0-disable) |
| ISR | 0:15a30802e719 | 91 | DigitalOut power_36khz (PTD0,0); //Power enable pic12f - 36khz (0- enable 1-disable) |
| ISR | 0:15a30802e719 | 92 | |
| ISR | 0:15a30802e719 | 93 | |
| ISR | 0:15a30802e719 | 94 | // ******************************************************************** |
| ISR | 0:15a30802e719 | 95 | // ******************************************************************** |
| ISR | 0:15a30802e719 | 96 | //DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT |
| ISR | 0:15a30802e719 | 97 | //ENTRADAS DIGITAIS (normal input) |
| ISR | 0:15a30802e719 | 98 | DigitalIn i_enc_dir_rig (PTB8); //Encoder Right Direction |
| ISR | 0:15a30802e719 | 99 | DigitalIn i_enc_dir_lef (PTB9); //Encoder Left Direction |
| ISR | 0:15a30802e719 | 100 | DigitalIn i_micro_sd_det (PTC16); //MICRO SD Card Detect |
| ISR | 0:15a30802e719 | 101 | DigitalIn i_mppt_fail (PTE5); //Fail MPPT Signal |
| ISR | 0:15a30802e719 | 102 | DigitalIn i_usb_volt (PTB10); //USB Voltage detect |
| ISR | 0:15a30802e719 | 103 | DigitalIn i_sup_cap_est (PTB19); //Supercap State Charger |
| ISR | 0:15a30802e719 | 104 | DigitalIn i_li_ion_est (PTB18); //Li-ion State Charger |
| ISR | 0:15a30802e719 | 105 | |
| ISR | 0:15a30802e719 | 106 | |
| ISR | 0:15a30802e719 | 107 | // ******************************************************************** |
| ISR | 0:15a30802e719 | 108 | //ENTRADAS DIGITAIS (external interrupt) |
| ISR | 0:15a30802e719 | 109 | InterruptIn i_int_mpu9250 (PTA15); //Interrupt MPU9250 |
| ISR | 0:15a30802e719 | 110 | InterruptIn i_int_isl29125 (PTA16); //Interrupt ISL29125 Color S. |
| ISR | 0:15a30802e719 | 111 | InterruptIn i_mic_f_l (PTD7); //Interrupt Comp Micro F L |
| ISR | 0:15a30802e719 | 112 | InterruptIn i_mic_f_r (PTD6); //Interrupt Comp Micro F R |
| ISR | 0:15a30802e719 | 113 | InterruptIn i_mic_r_c (PTD5); //Interrupt Comp Micro R C |
| ISR | 0:15a30802e719 | 114 | |
| ISR | 0:15a30802e719 | 115 | |
| ISR | 0:15a30802e719 | 116 | // ******************************************************************** |
| ISR | 0:15a30802e719 | 117 | //ENTRADAS ANALOGICAS |
| ISR | 0:15a30802e719 | 118 | AnalogIn a_enc_rig (PTC2); //Encoder Left Output_AS_MR |
| ISR | 0:15a30802e719 | 119 | AnalogIn a_enc_lef (PTC1); //Encoder Right Output_AS_ML |
| ISR | 0:15a30802e719 | 120 | AnalogIn a_mic_f_l (PTB0); //Analog microphone F L |
| ISR | 0:15a30802e719 | 121 | AnalogIn a_mic_f_r (PTB1); //Analog microphone F R |
| ISR | 0:15a30802e719 | 122 | AnalogIn a_mic_r_c (PTB2); //Analog microphone R C |
| ISR | 0:15a30802e719 | 123 | AnalogIn a_temp_bat (PTB3); //Temperature Battery |
| ISR | 0:15a30802e719 | 124 | |
| ISR | 0:15a30802e719 | 125 | |
| ISR | 0:15a30802e719 | 126 | // ******************************************************************** |
| ISR | 0:15a30802e719 | 127 | |
| ISR | 0:15a30802e719 | 128 | //PWM OR DIGITAL OUTPUT NORMAL |
| ISR | 0:15a30802e719 | 129 | //DigitalOut q_led_whi (PTE29); //Led white pwm |
| ISR | 0:15a30802e719 | 130 | DigitalOut q_led_red_fro (PTA4); //Led Red Front |
| ISR | 0:15a30802e719 | 131 | DigitalOut q_led_gre_fro (PTA5); //Led Green Front |
| ISR | 0:15a30802e719 | 132 | DigitalOut q_led_blu_fro (PTA12); //Led Blue Front |
| ISR | 0:15a30802e719 | 133 | DigitalOut q_led_red_rea (PTD4); //Led Red Rear |
| ISR | 0:15a30802e719 | 134 | DigitalOut q_led_gre_rea (PTA1); //Led Green Rear |
| ISR | 0:15a30802e719 | 135 | DigitalOut q_led_blu_rea (PTA2); //Led Blue Rear |
| ISR | 0:15a30802e719 | 136 | |
| ISR | 0:15a30802e719 | 137 | |
| ISR | 0:15a30802e719 | 138 | //SAIDAS DIGITAIS (pwm) |
| ISR | 0:15a30802e719 | 139 | PwmOut pwm_mot_rig (PTE20); //PWM Enable Motor Right |
| ISR | 0:15a30802e719 | 140 | PwmOut pwm_mot_lef (PTE31); //PWM Enable Motor Left |
| ISR | 0:15a30802e719 | 141 | PwmOut pwm_buzzer (PTE21); //Buzzer PWM |
| ISR | 0:15a30802e719 | 142 | PwmOut pwm_led_whi (PTE29); //Led white pwm |
| ISR | 0:15a30802e719 | 143 | |
| ISR | 0:15a30802e719 | 144 | // ******************************************************************** |
| ISR | 0:15a30802e719 | 145 | //SAIDAS ANALOGICAS |
| ISR | 0:15a30802e719 | 146 | AnalogOut dac_comp_mic (PTE30); //Dac_Comparator MIC |
| ISR | 0:15a30802e719 | 147 | |
| ISR | 0:15a30802e719 | 148 | |
| ISR | 1:8569ac717e68 | 149 | |
| ISR | 0:15a30802e719 | 150 | |
| ISR | 0:15a30802e719 | 151 | /** |
| ISR | 0:15a30802e719 | 152 | * Selects the wich infrared to comunicate. |
| ISR | 0:15a30802e719 | 153 | * |
| ISR | 0:15a30802e719 | 154 | * @param ch - Infrared to read (1..5) |
| ISR | 0:15a30802e719 | 155 | */ |
| ISR | 0:15a30802e719 | 156 | void tca9548_select_ch(char ch) |
| ISR | 0:15a30802e719 | 157 | { |
| ISR | 0:15a30802e719 | 158 | char ch_f[1]; |
| ISR | 0:15a30802e719 | 159 | char addr=0xE0; |
| ISR | 0:15a30802e719 | 160 | |
| ISR | 0:15a30802e719 | 161 | if(ch==0) |
| ISR | 0:15a30802e719 | 162 | ch_f[0]=1; |
| ISR | 0:15a30802e719 | 163 | |
| ISR | 0:15a30802e719 | 164 | if(ch>=1) |
| ISR | 0:15a30802e719 | 165 | ch_f[0]=1<<ch; |
| ISR | 0:15a30802e719 | 166 | |
| ISR | 1:8569ac717e68 | 167 | mutex_i2c0.lock(); |
| ISR | 0:15a30802e719 | 168 | i2c.write(addr,ch_f,1); |
| ISR | 1:8569ac717e68 | 169 | mutex_i2c0.unlock(); |
| ISR | 1:8569ac717e68 | 170 | } |
| ISR | 1:8569ac717e68 | 171 | |
| ISR | 1:8569ac717e68 | 172 | |
| ISR | 1:8569ac717e68 | 173 | |
| ISR | 1:8569ac717e68 | 174 | |
| ISR | 1:8569ac717e68 | 175 | |
| ISR | 1:8569ac717e68 | 176 | /* Powers up all the VCNL4020. */ |
| ISR | 1:8569ac717e68 | 177 | void init_Infrared() |
| ISR | 1:8569ac717e68 | 178 | { |
| ISR | 1:8569ac717e68 | 179 | |
| ISR | 1:8569ac717e68 | 180 | for (int i=0; i<6;i++) |
| ISR | 1:8569ac717e68 | 181 | { |
| ISR | 1:8569ac717e68 | 182 | tca9548_select_ch(i); |
| ISR | 1:8569ac717e68 | 183 | VCNL40x0_Device.SetCurrent (20); // Set current to 200mA |
| ISR | 1:8569ac717e68 | 184 | } |
| ISR | 1:8569ac717e68 | 185 | tca9548_select_ch(0); |
| ISR | 0:15a30802e719 | 186 | } |
| ISR | 0:15a30802e719 | 187 | |
| ISR | 0:15a30802e719 | 188 | |
| ISR | 0:15a30802e719 | 189 | /** |
| ISR | 1:8569ac717e68 | 190 | * Get the distance from of the chosen infrared. |
| ISR | 0:15a30802e719 | 191 | * |
| ISR | 1:8569ac717e68 | 192 | * @param ch - Infrared to read (0,1..5) |
| ISR | 0:15a30802e719 | 193 | * |
| ISR | 0:15a30802e719 | 194 | * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back) |
| ISR | 0:15a30802e719 | 195 | */ |
| ISR | 1:8569ac717e68 | 196 | float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras |
| ISR | 0:15a30802e719 | 197 | { |
| ISR | 0:15a30802e719 | 198 | tca9548_select_ch(ch); |
| ISR | 0:15a30802e719 | 199 | VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue); // read prox value on demand |
| ISR | 1:8569ac717e68 | 200 | float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ; |
| ISR | 1:8569ac717e68 | 201 | return aux; |
| ISR | 1:8569ac717e68 | 202 | //return ProxiValue; |
| ISR | 0:15a30802e719 | 203 | } |
| ISR | 0:15a30802e719 | 204 | |
| ISR | 0:15a30802e719 | 205 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 206 | /////////////////////////////////// MOTOR /////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 207 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 208 | |
| ISR | 0:15a30802e719 | 209 | // 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 | 210 | // consultar pag 39 e 95 |
| ISR | 0:15a30802e719 | 211 | |
| ISR | 0:15a30802e719 | 212 | /** |
| ISR | 0:15a30802e719 | 213 | * Sets speed and direction of the left motor. |
| ISR | 0:15a30802e719 | 214 | * |
| ISR | 0:15a30802e719 | 215 | * @param Dir - Direction of movement, 0 for back, or 1 for fron |
| ISR | 0:15a30802e719 | 216 | * @param Speed - Percentage of speed of the motor (1..100) |
| ISR | 0:15a30802e719 | 217 | * |
| ISR | 0:15a30802e719 | 218 | * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back |
| ISR | 0:15a30802e719 | 219 | * at different speeds and see if it makes a straigth line |
| ISR | 0:15a30802e719 | 220 | */ |
| ISR | 0:15a30802e719 | 221 | void leftMotor(short int Dir,short int Speed) |
| ISR | 0:15a30802e719 | 222 | { |
| ISR | 0:15a30802e719 | 223 | float Duty; |
| ISR | 0:15a30802e719 | 224 | |
| ISR | 0:15a30802e719 | 225 | if(Dir==1) { |
| ISR | 0:15a30802e719 | 226 | q_pha_mot_lef=0; //Andar em frente |
| ISR | 0:15a30802e719 | 227 | if(Speed>1000) //limite de segurança |
| ISR | 0:15a30802e719 | 228 | Speed=1000; |
| ISR | 0:15a30802e719 | 229 | if(Speed>0) { |
| ISR | 0:15a30802e719 | 230 | Duty=Speed*.082 +35; // 35 = minimo para o motor rodar |
| ISR | 0:15a30802e719 | 231 | q_sleep_mot_lef=1; //Nano Sleep Motor Left |
| ISR | 0:15a30802e719 | 232 | pwm_mot_lef.pulsewidth_us(Duty*5); |
| ISR | 0:15a30802e719 | 233 | } else { |
| ISR | 0:15a30802e719 | 234 | q_sleep_mot_lef=0; |
| ISR | 0:15a30802e719 | 235 | } |
| ISR | 0:15a30802e719 | 236 | } |
| ISR | 0:15a30802e719 | 237 | if(Dir==0) { |
| ISR | 0:15a30802e719 | 238 | q_pha_mot_lef=1; //Andar para tras |
| ISR | 0:15a30802e719 | 239 | |
| ISR | 0:15a30802e719 | 240 | if(Speed>1000) //limite de segurança |
| ISR | 0:15a30802e719 | 241 | Speed=1000; |
| ISR | 0:15a30802e719 | 242 | if(Speed>0) { |
| ISR | 0:15a30802e719 | 243 | Duty=Speed*.082 +35; // 35 = minimo para o motor rodar |
| ISR | 0:15a30802e719 | 244 | q_sleep_mot_lef=1; //Nano Sleep Motor Left |
| ISR | 0:15a30802e719 | 245 | pwm_mot_lef.pulsewidth_us(Duty*5); |
| ISR | 0:15a30802e719 | 246 | } else { |
| ISR | 0:15a30802e719 | 247 | q_sleep_mot_lef=0; |
| ISR | 0:15a30802e719 | 248 | } |
| ISR | 0:15a30802e719 | 249 | } |
| ISR | 0:15a30802e719 | 250 | } |
| ISR | 0:15a30802e719 | 251 | |
| ISR | 0:15a30802e719 | 252 | |
| ISR | 0:15a30802e719 | 253 | /** |
| ISR | 0:15a30802e719 | 254 | * Sets speed and direction of the right motor. |
| ISR | 0:15a30802e719 | 255 | * |
| ISR | 0:15a30802e719 | 256 | * @param Dir - Direction of movement, 0 for back, or 1 for fron |
| ISR | 0:15a30802e719 | 257 | * @param Speed - Percentage of speed of the motor (1..100) |
| ISR | 0:15a30802e719 | 258 | * |
| ISR | 0:15a30802e719 | 259 | * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back |
| ISR | 0:15a30802e719 | 260 | * at different speeds and see if it makes a straigth line |
| ISR | 0:15a30802e719 | 261 | */ |
| ISR | 0:15a30802e719 | 262 | void rightMotor(short int Dir,short int Speed) |
| ISR | 0:15a30802e719 | 263 | { |
| ISR | 0:15a30802e719 | 264 | float Duty; |
| ISR | 0:15a30802e719 | 265 | |
| ISR | 0:15a30802e719 | 266 | if(Dir==1) { |
| ISR | 0:15a30802e719 | 267 | q_pha_mot_rig=0; //Andar em frente |
| ISR | 0:15a30802e719 | 268 | |
| ISR | 0:15a30802e719 | 269 | if(Speed>1000) //limite de segurança |
| ISR | 0:15a30802e719 | 270 | Speed=1000; |
| ISR | 0:15a30802e719 | 271 | if(Speed>0) { |
| ISR | 0:15a30802e719 | 272 | Duty=Speed*.082 +35; // 35 = minimo para o motor rodar |
| ISR | 0:15a30802e719 | 273 | q_sleep_mot_rig=1; //Nano Sleep Motor Right |
| ISR | 0:15a30802e719 | 274 | pwm_mot_rig.pulsewidth_us(Duty*5); |
| ISR | 0:15a30802e719 | 275 | } else { |
| ISR | 0:15a30802e719 | 276 | q_sleep_mot_rig=0; |
| ISR | 0:15a30802e719 | 277 | } |
| ISR | 0:15a30802e719 | 278 | } |
| ISR | 0:15a30802e719 | 279 | if(Dir==0) { |
| ISR | 0:15a30802e719 | 280 | q_pha_mot_rig=1; //Andar para tras |
| ISR | 0:15a30802e719 | 281 | |
| ISR | 0:15a30802e719 | 282 | |
| ISR | 0:15a30802e719 | 283 | if(Speed>1000) //limite de segurança |
| ISR | 0:15a30802e719 | 284 | Speed=1000; |
| ISR | 0:15a30802e719 | 285 | if(Speed>0) { |
| ISR | 0:15a30802e719 | 286 | Duty=Speed*.082 +35; // 35 = minimo para o motor rodar |
| ISR | 0:15a30802e719 | 287 | q_sleep_mot_rig=1; //Nano Sleep Motor Right |
| ISR | 0:15a30802e719 | 288 | pwm_mot_rig.pulsewidth_us(Duty*5); |
| ISR | 0:15a30802e719 | 289 | } else { |
| ISR | 0:15a30802e719 | 290 | q_sleep_mot_rig=0; |
| ISR | 0:15a30802e719 | 291 | } |
| ISR | 0:15a30802e719 | 292 | } |
| ISR | 0:15a30802e719 | 293 | } |
| ISR | 0:15a30802e719 | 294 | |
| ISR | 0:15a30802e719 | 295 | |
| ISR | 0:15a30802e719 | 296 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 297 | /////////////////////////////////// BATTERY /////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 298 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 299 | |
| ISR | 0:15a30802e719 | 300 | /** |
| ISR | 0:15a30802e719 | 301 | * Reads adc of the battery. |
| ISR | 0:15a30802e719 | 302 | * |
| ISR | 0:15a30802e719 | 303 | * @param addr - Address to read |
| ISR | 1:8569ac717e68 | 304 | * @return The voltage of the baterry |
| ISR | 0:15a30802e719 | 305 | */ |
| ISR | 0:15a30802e719 | 306 | long int read16_mcp3424(char addr) |
| ISR | 0:15a30802e719 | 307 | { |
| ISR | 0:15a30802e719 | 308 | char data[4]; |
| ISR | 1:8569ac717e68 | 309 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 310 | i2c1.read(addr,data,3); |
| ISR | 1:8569ac717e68 | 311 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 312 | return(((data[0]&127)*256)+data[1]); |
| ISR | 0:15a30802e719 | 313 | } |
| ISR | 0:15a30802e719 | 314 | |
| ISR | 0:15a30802e719 | 315 | /** |
| ISR | 0:15a30802e719 | 316 | * Reads adc of the battery. |
| ISR | 0:15a30802e719 | 317 | * |
| ISR | 0:15a30802e719 | 318 | * @param n_bits - Resolution of measure |
| ISR | 0:15a30802e719 | 319 | * @param ch - Chose value to read, if voltage or current of solar or batery |
| ISR | 0:15a30802e719 | 320 | * @param gain - |
| ISR | 0:15a30802e719 | 321 | * @param addr - Address to write to |
| ISR | 0:15a30802e719 | 322 | */ |
| ISR | 0:15a30802e719 | 323 | void write_mcp3424(int n_bits, int ch, int gain, char addr) //chanel 1-4 write -> 0xD0 |
| ISR | 0:15a30802e719 | 324 | { |
| ISR | 0:15a30802e719 | 325 | |
| ISR | 0:15a30802e719 | 326 | int chanel_end=(ch-1)<<5; //shift left |
| ISR | 0:15a30802e719 | 327 | char n_bits_end=0; |
| ISR | 0:15a30802e719 | 328 | |
| ISR | 0:15a30802e719 | 329 | if(n_bits==12) { |
| ISR | 0:15a30802e719 | 330 | n_bits_end=0; |
| ISR | 0:15a30802e719 | 331 | } else if(n_bits==14) { |
| ISR | 0:15a30802e719 | 332 | n_bits_end=1; |
| ISR | 0:15a30802e719 | 333 | } else if(n_bits==16) { |
| ISR | 0:15a30802e719 | 334 | n_bits_end=2; |
| ISR | 0:15a30802e719 | 335 | } else { |
| ISR | 0:15a30802e719 | 336 | n_bits_end=3; |
| ISR | 0:15a30802e719 | 337 | } |
| ISR | 0:15a30802e719 | 338 | n_bits_end=n_bits_end<<2; //shift left |
| ISR | 0:15a30802e719 | 339 | |
| ISR | 0:15a30802e719 | 340 | char data[1]; |
| ISR | 0:15a30802e719 | 341 | data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128; |
| ISR | 1:8569ac717e68 | 342 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 343 | i2c1.write(addr,data,1); |
| ISR | 1:8569ac717e68 | 344 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 345 | } |
| ISR | 0:15a30802e719 | 346 | |
| ISR | 0:15a30802e719 | 347 | |
| ISR | 0:15a30802e719 | 348 | /** |
| ISR | 0:15a30802e719 | 349 | * Reads adc of the battery. |
| ISR | 0:15a30802e719 | 350 | * |
| ISR | 0:15a30802e719 | 351 | * @return The voltage of the batery |
| ISR | 0:15a30802e719 | 352 | */ |
| ISR | 0:15a30802e719 | 353 | float value_of_Batery() |
| ISR | 0:15a30802e719 | 354 | { |
| ISR | 0:15a30802e719 | 355 | float R1=75000.0; |
| ISR | 0:15a30802e719 | 356 | float R2=39200.0; |
| ISR | 0:15a30802e719 | 357 | float R3=178000.0; |
| ISR | 0:15a30802e719 | 358 | float Gain=1.0; |
| ISR | 0:15a30802e719 | 359 | write_mcp3424(16,3,1,0xd8); |
| ISR | 0:15a30802e719 | 360 | float cha3_v2=read16_mcp3424(0xd9); //read voltage |
| ISR | 0:15a30802e719 | 361 | float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain; |
| ISR | 0:15a30802e719 | 362 | float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2)); |
| ISR | 0:15a30802e719 | 363 | Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268; |
| ISR | 0:15a30802e719 | 364 | |
| ISR | 0:15a30802e719 | 365 | return Vin_b_v_battery; |
| ISR | 0:15a30802e719 | 366 | } |
| ISR | 0:15a30802e719 | 367 | |
| ISR | 0:15a30802e719 | 368 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 369 | ////////////////////////////////// Sonar //////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 370 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 371 | // Commands of operation with ultrasonic module |
| ISR | 0:15a30802e719 | 372 | |
| ISR | 0:15a30802e719 | 373 | // WRITE OPTION: |
| ISR | 0:15a30802e719 | 374 | // ENABLE DC DC CONVERTER - 0x0C; |
| ISR | 0:15a30802e719 | 375 | // DISABLE DC DC CONVERTER - 0x0B; |
| ISR | 0:15a30802e719 | 376 | // START MEASURE LEFT SENSOR - 0x0A; |
| ISR | 0:15a30802e719 | 377 | // START MEASURE FRONT SENSOR - 0x09; |
| ISR | 0:15a30802e719 | 378 | // START MEASURE RIGHT SENSOR - 0x08; |
| ISR | 0:15a30802e719 | 379 | // SENSORS ALWAYS MEASURE ON - 0x07; |
| ISR | 0:15a30802e719 | 380 | // SENSORS ALWAYS MEASURE OFF - 0x06; |
| ISR | 0:15a30802e719 | 381 | |
| ISR | 0:15a30802e719 | 382 | // READ OPTION: |
| ISR | 0:15a30802e719 | 383 | // GET MEASURE OF LEFT SENSOR - 0x05; |
| ISR | 0:15a30802e719 | 384 | // GET MEASURE OF FRONT SENSOR - 0x04; |
| ISR | 0:15a30802e719 | 385 | // GET MEASURE OF IGHT SENSOR - 0x03; |
| ISR | 0:15a30802e719 | 386 | // GET STATUS SENSORS ALWAYS MEASURE - 0x02; |
| ISR | 0:15a30802e719 | 387 | // GET STATUS DC DC CONVERTER - 0x01; |
| ISR | 0:15a30802e719 | 388 | |
| ISR | 0:15a30802e719 | 389 | void enable_dc_dc_boost() |
| ISR | 0:15a30802e719 | 390 | { |
| ISR | 0:15a30802e719 | 391 | char data[1]; |
| ISR | 0:15a30802e719 | 392 | data[0]= 0x0C; |
| ISR | 1:8569ac717e68 | 393 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 394 | i2c1.write(0x30,data,1); |
| ISR | 1:8569ac717e68 | 395 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 396 | } |
| ISR | 0:15a30802e719 | 397 | |
| ISR | 0:15a30802e719 | 398 | |
| ISR | 0:15a30802e719 | 399 | void measure_always_on() // left, front, right |
| ISR | 0:15a30802e719 | 400 | { |
| ISR | 0:15a30802e719 | 401 | char data[1]; |
| ISR | 0:15a30802e719 | 402 | data[0]= 0x07; |
| ISR | 1:8569ac717e68 | 403 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 404 | i2c1.write(0x30,data,1); |
| ISR | 1:8569ac717e68 | 405 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 406 | } |
| ISR | 0:15a30802e719 | 407 | |
| ISR | 0:15a30802e719 | 408 | |
| ISR | 0:15a30802e719 | 409 | /** |
| ISR | 0:15a30802e719 | 410 | * Returns left sensor value |
| ISR | 0:15a30802e719 | 411 | */ |
| ISR | 0:15a30802e719 | 412 | static unsigned int get_distance_left_sensor() |
| ISR | 0:15a30802e719 | 413 | { |
| ISR | 1:8569ac717e68 | 414 | |
| ISR | 0:15a30802e719 | 415 | static char data_r[3]; |
| ISR | 0:15a30802e719 | 416 | static unsigned int aux; |
| ISR | 1:8569ac717e68 | 417 | |
| ISR | 0:15a30802e719 | 418 | data_r[0]= 0x05; |
| ISR | 1:8569ac717e68 | 419 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 420 | i2c1.write(0x30,data_r,1); |
| ISR | 1:8569ac717e68 | 421 | i2c1.read(0x31,data_r,2); |
| ISR | 1:8569ac717e68 | 422 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 423 | |
| ISR | 0:15a30802e719 | 424 | aux=(data_r[0]*256)+data_r[1]; |
| ISR | 1:8569ac717e68 | 425 | |
| ISR | 0:15a30802e719 | 426 | return aux; |
| ISR | 0:15a30802e719 | 427 | } |
| ISR | 0:15a30802e719 | 428 | |
| ISR | 0:15a30802e719 | 429 | |
| ISR | 0:15a30802e719 | 430 | /** |
| ISR | 0:15a30802e719 | 431 | * Returns front sensor value |
| ISR | 0:15a30802e719 | 432 | */ |
| ISR | 0:15a30802e719 | 433 | static unsigned int get_distance_front_sensor() |
| ISR | 0:15a30802e719 | 434 | { |
| ISR | 0:15a30802e719 | 435 | |
| ISR | 0:15a30802e719 | 436 | static char data_r[3]; |
| ISR | 0:15a30802e719 | 437 | static unsigned int aux; |
| ISR | 1:8569ac717e68 | 438 | |
| ISR | 0:15a30802e719 | 439 | data_r[0]= 0x04; |
| ISR | 1:8569ac717e68 | 440 | |
| ISR | 1:8569ac717e68 | 441 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 442 | i2c1.write(0x30,data_r,1); |
| ISR | 1:8569ac717e68 | 443 | i2c1.read(0x31,data_r,2); |
| ISR | 1:8569ac717e68 | 444 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 445 | |
| ISR | 0:15a30802e719 | 446 | aux=(data_r[0]*256)+data_r[1]; |
| ISR | 1:8569ac717e68 | 447 | |
| ISR | 0:15a30802e719 | 448 | return aux; |
| ISR | 0:15a30802e719 | 449 | |
| ISR | 0:15a30802e719 | 450 | } |
| ISR | 0:15a30802e719 | 451 | |
| ISR | 0:15a30802e719 | 452 | |
| ISR | 0:15a30802e719 | 453 | /** |
| ISR | 0:15a30802e719 | 454 | * Returns right sensor value |
| ISR | 0:15a30802e719 | 455 | */ |
| ISR | 0:15a30802e719 | 456 | static unsigned int get_distance_right_sensor() |
| ISR | 0:15a30802e719 | 457 | { |
| ISR | 0:15a30802e719 | 458 | |
| ISR | 0:15a30802e719 | 459 | static char data_r[3]; |
| ISR | 0:15a30802e719 | 460 | static unsigned int aux; |
| ISR | 0:15a30802e719 | 461 | |
| ISR | 0:15a30802e719 | 462 | data_r[0]= 0x03; |
| ISR | 1:8569ac717e68 | 463 | |
| ISR | 1:8569ac717e68 | 464 | mutex_i2c1.lock(); |
| ISR | 0:15a30802e719 | 465 | i2c1.write(0x30,data_r,1); |
| ISR | 0:15a30802e719 | 466 | i2c1.read(0x31,data_r,2,0); |
| ISR | 1:8569ac717e68 | 467 | mutex_i2c1.unlock(); |
| ISR | 0:15a30802e719 | 468 | |
| ISR | 0:15a30802e719 | 469 | aux=(data_r[0]*256)+data_r[1]; |
| ISR | 1:8569ac717e68 | 470 | |
| ISR | 0:15a30802e719 | 471 | return aux; |
| ISR | 0:15a30802e719 | 472 | |
| ISR | 0:15a30802e719 | 473 | } |
| ISR | 0:15a30802e719 | 474 | |
| ISR | 0:15a30802e719 | 475 | |
| ISR | 1:8569ac717e68 | 476 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 1:8569ac717e68 | 477 | ////////////////////////////////// RF COMMUNICATION //////////////////////////////////////////// |
| ISR | 1:8569ac717e68 | 478 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 479 | |
| ISR | 1:8569ac717e68 | 480 | /** |
| ISR | 1:8569ac717e68 | 481 | * Initializes nrf24l01 module, return value 1 if module is working correcty and 0 if it's not. |
| ISR | 1:8569ac717e68 | 482 | * |
| ISR | 1:8569ac717e68 | 483 | ** @param channel - Which RF channel to comunicate on, 0-125 |
| ISR | 1:8569ac717e68 | 484 | * |
| ISR | 1:8569ac717e68 | 485 | * \warning Channel on Robot and Board has to be the same. |
| ISR | 1:8569ac717e68 | 486 | */ |
| ISR | 1:8569ac717e68 | 487 | void init_nRF(int channel){ |
| ISR | 1:8569ac717e68 | 488 | int result; |
| ISR | 1:8569ac717e68 | 489 | result = radio.begin(); |
| ISR | 1:8569ac717e68 | 490 | |
| ISR | 1:8569ac717e68 | 491 | pc.printf( "Initialation nrf24l01=%d\r\n", result ); // 1-working,0-not working |
| ISR | 1:8569ac717e68 | 492 | radio.setDataRate(RF24_1MBPS); |
| ISR | 1:8569ac717e68 | 493 | radio.setCRCLength(RF24_CRC_16); |
| ISR | 1:8569ac717e68 | 494 | radio.setPayloadSize(32); |
| ISR | 1:8569ac717e68 | 495 | radio.setChannel(channel); |
| ISR | 1:8569ac717e68 | 496 | radio.setAutoAck(true); |
| ISR | 1:8569ac717e68 | 497 | |
| ISR | 1:8569ac717e68 | 498 | radio.openWritingPipe(0x314e6f6465); |
| ISR | 1:8569ac717e68 | 499 | radio.openReadingPipe(1,0x324e6f6465); |
| ISR | 1:8569ac717e68 | 500 | |
| ISR | 1:8569ac717e68 | 501 | radio.startListening(); |
| ISR | 1:8569ac717e68 | 502 | } |
| ISR | 1:8569ac717e68 | 503 | |
| ISR | 1:8569ac717e68 | 504 | char txData, rxData; |
| ISR | 1:8569ac717e68 | 505 | char q[1]; |
| ISR | 1:8569ac717e68 | 506 | |
| ISR | 0:15a30802e719 | 507 | |
| ISR | 1:8569ac717e68 | 508 | /** |
| ISR | 1:8569ac717e68 | 509 | * Wireless control of motors using nrf24l01 module and FRDM-KL25Z board(robot is working as receiver). |
| ISR | 1:8569ac717e68 | 510 | */ |
| ISR | 1:8569ac717e68 | 511 | void robot_RC() |
| ISR | 1:8569ac717e68 | 512 | { |
| ISR | 1:8569ac717e68 | 513 | if (radio.available() ) { |
| ISR | 0:15a30802e719 | 514 | |
| ISR | 1:8569ac717e68 | 515 | radio.read( &rxData, sizeof(rxData) ); |
| ISR | 1:8569ac717e68 | 516 | pc.putc( rxData ); |
| ISR | 1:8569ac717e68 | 517 | |
| ISR | 1:8569ac717e68 | 518 | q[0]=pc.putc( rxData ); |
| ISR | 1:8569ac717e68 | 519 | if (q[0]=='w'){ |
| ISR | 1:8569ac717e68 | 520 | leftMotor(1,500); |
| ISR | 1:8569ac717e68 | 521 | rightMotor(1,500); |
| ISR | 1:8569ac717e68 | 522 | } |
| ISR | 1:8569ac717e68 | 523 | else if( q[0]=='s'){ |
| ISR | 1:8569ac717e68 | 524 | leftMotor(0,500); |
| ISR | 1:8569ac717e68 | 525 | rightMotor(0,500); |
| ISR | 1:8569ac717e68 | 526 | } |
| ISR | 1:8569ac717e68 | 527 | else if( q[0]=='a'){ |
| ISR | 1:8569ac717e68 | 528 | leftMotor(0,500); |
| ISR | 1:8569ac717e68 | 529 | rightMotor(1,500); |
| ISR | 1:8569ac717e68 | 530 | } |
| ISR | 1:8569ac717e68 | 531 | else if( q[0]=='d'){ |
| ISR | 1:8569ac717e68 | 532 | leftMotor(1,500); |
| ISR | 1:8569ac717e68 | 533 | rightMotor(0,1000); |
| ISR | 1:8569ac717e68 | 534 | } |
| ISR | 1:8569ac717e68 | 535 | else if( q[0]==' '){ |
| ISR | 1:8569ac717e68 | 536 | leftMotor(1,0); |
| ISR | 1:8569ac717e68 | 537 | rightMotor(1,0); |
| ISR | 1:8569ac717e68 | 538 | } |
| ISR | 1:8569ac717e68 | 539 | } |
| ISR | 0:15a30802e719 | 540 | } |
| ISR | 0:15a30802e719 | 541 | |
| ISR | 0:15a30802e719 | 542 | |
| ISR | 1:8569ac717e68 | 543 | |
| ISR | 1:8569ac717e68 | 544 | /** |
| ISR | 1:8569ac717e68 | 545 | * Robot is sending values of all sensors (right/left incremental encoder, ultrasonic sensors, infarated sensors and Odometria) to FRDM-KL25Z board using nrf24l01 module. |
| ISR | 1:8569ac717e68 | 546 | * |
| ISR | 1:8569ac717e68 | 547 | *Note: Check if module is initilized correctly. |
| ISR | 1:8569ac717e68 | 548 | */ |
| ISR | 1:8569ac717e68 | 549 | void send_Robot_values(){ |
| ISR | 1:8569ac717e68 | 550 | |
| ISR | 1:8569ac717e68 | 551 | // WRITE ID: |
| ISR | 1:8569ac717e68 | 552 | // INCREMENTAL LEFT ENCODER - a |
| ISR | 1:8569ac717e68 | 553 | // INCREMENTAL RIGHT ENCODER - b |
| ISR | 1:8569ac717e68 | 554 | // LEFT ULTRASENSOR - c |
| ISR | 1:8569ac717e68 | 555 | // FRONT ULTRASENSOR - d |
| ISR | 1:8569ac717e68 | 556 | // RIGHT ULTRASENSOR - e |
| ISR | 1:8569ac717e68 | 557 | // INFRARED SENSOR 0 - f |
| ISR | 1:8569ac717e68 | 558 | // INFRARED SENSOR 1 - g |
| ISR | 1:8569ac717e68 | 559 | // INFRARED SENSOR 2 - h |
| ISR | 1:8569ac717e68 | 560 | // INFRARED SENSOR 3 - i |
| ISR | 1:8569ac717e68 | 561 | // INFRARED SENSOR 4 - j |
| ISR | 1:8569ac717e68 | 562 | // INFRARED SENSOR 5 - k |
| ISR | 1:8569ac717e68 | 563 | // ODOMETRIA "X" - l |
| ISR | 1:8569ac717e68 | 564 | // ODOMETRIA "Y" - m |
| ISR | 1:8569ac717e68 | 565 | |
| ISR | 1:8569ac717e68 | 566 | // LEFT ENCODER addr: a |
| ISR | 1:8569ac717e68 | 567 | |
| ISR | 1:8569ac717e68 | 568 | int left_encoder = 0; //read value from device |
| ISR | 1:8569ac717e68 | 569 | char left_encoder_R[10]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 570 | char left_encoder_W[12]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 571 | |
| ISR | 1:8569ac717e68 | 572 | snprintf(left_encoder_R, 10, "%d", left_encoder); //int value to char array |
| ISR | 1:8569ac717e68 | 573 | |
| ISR | 1:8569ac717e68 | 574 | for (int i=0; i < 10;i++) //changing posiision of chars in array |
| ISR | 1:8569ac717e68 | 575 | { |
| ISR | 1:8569ac717e68 | 576 | left_encoder_W[i+2]=left_encoder_R[i]; |
| ISR | 1:8569ac717e68 | 577 | } |
| ISR | 1:8569ac717e68 | 578 | |
| ISR | 1:8569ac717e68 | 579 | left_encoder_W[0]='Q'; //adding id to message |
| ISR | 1:8569ac717e68 | 580 | left_encoder_W[1]='a'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 581 | |
| ISR | 1:8569ac717e68 | 582 | // RIGHT ENCODER addr: b |
| ISR | 1:8569ac717e68 | 583 | |
| ISR | 1:8569ac717e68 | 584 | int right_encoder = 0; //read value from device |
| ISR | 1:8569ac717e68 | 585 | char right_encoder_R[10]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 586 | char right_encoder_W[11]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 587 | |
| ISR | 1:8569ac717e68 | 588 | snprintf(right_encoder_R, 10, "%d", right_encoder); //int value to char array |
| ISR | 1:8569ac717e68 | 589 | |
| ISR | 1:8569ac717e68 | 590 | for (int i=0; i <10;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 591 | { |
| ISR | 1:8569ac717e68 | 592 | right_encoder_W[i+1]=right_encoder_R[i]; |
| ISR | 1:8569ac717e68 | 593 | } |
| ISR | 1:8569ac717e68 | 594 | |
| ISR | 1:8569ac717e68 | 595 | right_encoder_W[0]='b'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 596 | |
| ISR | 1:8569ac717e68 | 597 | |
| ISR | 1:8569ac717e68 | 598 | // LEFT ULTRASENSOR addr: c |
| ISR | 1:8569ac717e68 | 599 | |
| ISR | 1:8569ac717e68 | 600 | int left_sensor = get_distance_left_sensor(); //read value from device |
| ISR | 1:8569ac717e68 | 601 | char left_sensor_R[5]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 602 | char left_sensor_W[6]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 603 | |
| ISR | 1:8569ac717e68 | 604 | snprintf(left_sensor_R, 5, "%d", left_sensor); //int value to char array |
| ISR | 0:15a30802e719 | 605 | |
| ISR | 1:8569ac717e68 | 606 | for (int i=0; i < 5;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 607 | { |
| ISR | 1:8569ac717e68 | 608 | left_sensor_W[i+1]=left_sensor_R[i]; |
| ISR | 1:8569ac717e68 | 609 | } |
| ISR | 1:8569ac717e68 | 610 | |
| ISR | 1:8569ac717e68 | 611 | left_sensor_W[0]='c'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 612 | |
| ISR | 1:8569ac717e68 | 613 | |
| ISR | 1:8569ac717e68 | 614 | // FRONT ULTRASENSOR addr: d |
| ISR | 1:8569ac717e68 | 615 | |
| ISR | 1:8569ac717e68 | 616 | int front_sensor = get_distance_front_sensor(); //read value from device |
| ISR | 1:8569ac717e68 | 617 | char front_sensor_R[5]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 618 | char front_sensor_W[6]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 619 | |
| ISR | 1:8569ac717e68 | 620 | snprintf(front_sensor_R, 5, "%d", front_sensor); //int value to char array |
| ISR | 1:8569ac717e68 | 621 | |
| ISR | 1:8569ac717e68 | 622 | for (int i=0; i < 5;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 623 | { |
| ISR | 1:8569ac717e68 | 624 | front_sensor_W[i+1]=front_sensor_R[i]; |
| ISR | 1:8569ac717e68 | 625 | } |
| ISR | 1:8569ac717e68 | 626 | |
| ISR | 1:8569ac717e68 | 627 | front_sensor_W[0]='d'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 628 | |
| ISR | 1:8569ac717e68 | 629 | |
| ISR | 1:8569ac717e68 | 630 | // RIGHT ULTRASENSOR addr: e |
| ISR | 1:8569ac717e68 | 631 | |
| ISR | 1:8569ac717e68 | 632 | int right_sensor = get_distance_right_sensor(); //read value from device |
| ISR | 1:8569ac717e68 | 633 | char right_sensor_R[5]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 634 | char right_sensor_W[6]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 635 | |
| ISR | 1:8569ac717e68 | 636 | snprintf(right_sensor_R, 5, "%d", right_sensor); //int value to char array |
| ISR | 1:8569ac717e68 | 637 | |
| ISR | 1:8569ac717e68 | 638 | for (int i=0; i < 5;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 639 | { |
| ISR | 1:8569ac717e68 | 640 | right_sensor_W[i+1]=right_sensor_R[i]; |
| ISR | 1:8569ac717e68 | 641 | } |
| ISR | 1:8569ac717e68 | 642 | |
| ISR | 1:8569ac717e68 | 643 | right_sensor_W[0]='e'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 644 | |
| ISR | 1:8569ac717e68 | 645 | |
| ISR | 1:8569ac717e68 | 646 | // INFRARED SENSOR 0 addr: f |
| ISR | 1:8569ac717e68 | 647 | |
| ISR | 1:8569ac717e68 | 648 | float infrared_0 = read_Infrared(0); //read value from device |
| ISR | 1:8569ac717e68 | 649 | |
| ISR | 1:8569ac717e68 | 650 | char infrared_0_R[5]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 651 | char infrared_0_W[7]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 652 | |
| ISR | 1:8569ac717e68 | 653 | snprintf(infrared_0_R, 5, "%.1f", infrared_0); //int value to char array |
| ISR | 1:8569ac717e68 | 654 | |
| ISR | 1:8569ac717e68 | 655 | for (int i=0; i < 6;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 656 | { |
| ISR | 1:8569ac717e68 | 657 | infrared_0_W[i+2]=infrared_0_R[i]; |
| ISR | 1:8569ac717e68 | 658 | } |
| ISR | 1:8569ac717e68 | 659 | |
| ISR | 1:8569ac717e68 | 660 | infrared_0_W[0]='Z'; //adding id to message |
| ISR | 1:8569ac717e68 | 661 | infrared_0_W[1]='f'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 662 | |
| ISR | 1:8569ac717e68 | 663 | // INFRARED SENSOR 1 addr: g |
| ISR | 1:8569ac717e68 | 664 | |
| ISR | 1:8569ac717e68 | 665 | float infrared_1 = read_Infrared(1); //read value from device |
| ISR | 1:8569ac717e68 | 666 | |
| ISR | 1:8569ac717e68 | 667 | char infrared_1_R[sizeof(infrared_1)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 668 | char infrared_1_W[sizeof(infrared_1)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 669 | |
| ISR | 1:8569ac717e68 | 670 | snprintf(infrared_1_R, sizeof(infrared_1_R), "%.1f", infrared_1); //int value to char array |
| ISR | 1:8569ac717e68 | 671 | |
| ISR | 1:8569ac717e68 | 672 | for (int i=0; i < sizeof(infrared_1)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 673 | { |
| ISR | 1:8569ac717e68 | 674 | infrared_1_W[i+1]=infrared_1_R[i]; |
| ISR | 1:8569ac717e68 | 675 | } |
| ISR | 1:8569ac717e68 | 676 | |
| ISR | 1:8569ac717e68 | 677 | infrared_1_W[0]='g'; //adding id to parameter |
| ISR | 0:15a30802e719 | 678 | |
| ISR | 1:8569ac717e68 | 679 | |
| ISR | 1:8569ac717e68 | 680 | // INFRARED SENSOR 2 addr: h |
| ISR | 1:8569ac717e68 | 681 | |
| ISR | 1:8569ac717e68 | 682 | float infrared_2 = read_Infrared(2); //read value from device |
| ISR | 1:8569ac717e68 | 683 | |
| ISR | 1:8569ac717e68 | 684 | char infrared_2_R[sizeof(infrared_2)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 685 | char infrared_2_W[sizeof(infrared_2)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 686 | |
| ISR | 1:8569ac717e68 | 687 | snprintf(infrared_2_R, sizeof(infrared_2_R), "%.1f", infrared_2); //int value to char array |
| ISR | 1:8569ac717e68 | 688 | |
| ISR | 1:8569ac717e68 | 689 | for (int i=0; i < sizeof(infrared_2)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 690 | { |
| ISR | 1:8569ac717e68 | 691 | infrared_2_W[i+1]=infrared_2_R[i]; |
| ISR | 1:8569ac717e68 | 692 | } |
| ISR | 1:8569ac717e68 | 693 | |
| ISR | 1:8569ac717e68 | 694 | infrared_2_W[0]='h'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 695 | |
| ISR | 1:8569ac717e68 | 696 | |
| ISR | 1:8569ac717e68 | 697 | // INFRARED SENSOR 3 addr: i |
| ISR | 1:8569ac717e68 | 698 | |
| ISR | 1:8569ac717e68 | 699 | float infrared_3 = read_Infrared(3); //read value from device |
| ISR | 1:8569ac717e68 | 700 | |
| ISR | 1:8569ac717e68 | 701 | char infrared_3_R[sizeof(infrared_3)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 702 | char infrared_3_W[sizeof(infrared_3)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 703 | |
| ISR | 1:8569ac717e68 | 704 | snprintf(infrared_3_R, sizeof(infrared_3_R), "%.1f", infrared_3); //int value to char array |
| ISR | 1:8569ac717e68 | 705 | |
| ISR | 1:8569ac717e68 | 706 | for (int i=0; i < sizeof(infrared_3)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 707 | { |
| ISR | 1:8569ac717e68 | 708 | infrared_3_W[i+1]=infrared_3_R[i]; |
| ISR | 1:8569ac717e68 | 709 | } |
| ISR | 1:8569ac717e68 | 710 | |
| ISR | 1:8569ac717e68 | 711 | infrared_3_W[0]='i'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 712 | |
| ISR | 1:8569ac717e68 | 713 | |
| ISR | 1:8569ac717e68 | 714 | // INFRARED SENSOR 4 addr: j |
| ISR | 1:8569ac717e68 | 715 | |
| ISR | 1:8569ac717e68 | 716 | float infrared_4 = read_Infrared(4); //read value from device |
| ISR | 1:8569ac717e68 | 717 | |
| ISR | 1:8569ac717e68 | 718 | char infrared_4_R[sizeof(infrared_4)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 719 | char infrared_4_W[sizeof(infrared_4)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 720 | |
| ISR | 1:8569ac717e68 | 721 | snprintf(infrared_4_R, sizeof(infrared_4_R), "%.1f", infrared_4); //int value to char array |
| ISR | 1:8569ac717e68 | 722 | |
| ISR | 1:8569ac717e68 | 723 | for (int i=0; i < sizeof(infrared_4)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 724 | { |
| ISR | 1:8569ac717e68 | 725 | infrared_4_W[i+1]=infrared_4_R[i]; |
| ISR | 1:8569ac717e68 | 726 | } |
| ISR | 1:8569ac717e68 | 727 | |
| ISR | 1:8569ac717e68 | 728 | infrared_4_W[0]='j'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 729 | |
| ISR | 1:8569ac717e68 | 730 | |
| ISR | 1:8569ac717e68 | 731 | // INFRARED SENSOR 5 addr: k |
| ISR | 1:8569ac717e68 | 732 | |
| ISR | 1:8569ac717e68 | 733 | float infrared_5 = read_Infrared(5); //read value from device |
| ISR | 1:8569ac717e68 | 734 | |
| ISR | 1:8569ac717e68 | 735 | char infrared_5_R[sizeof(infrared_5)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 736 | char infrared_5_W[sizeof(infrared_5)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 737 | |
| ISR | 1:8569ac717e68 | 738 | snprintf(infrared_5_R, sizeof(infrared_5_R), "%.1f", infrared_5); //int value to char array |
| ISR | 0:15a30802e719 | 739 | |
| ISR | 1:8569ac717e68 | 740 | for (int i=0; i < sizeof(infrared_5)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 741 | { |
| ISR | 1:8569ac717e68 | 742 | infrared_5_W[i+1]=infrared_5_R[i]; |
| ISR | 1:8569ac717e68 | 743 | } |
| ISR | 1:8569ac717e68 | 744 | |
| ISR | 1:8569ac717e68 | 745 | infrared_5_W[0]='k'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 746 | |
| ISR | 1:8569ac717e68 | 747 | |
| ISR | 1:8569ac717e68 | 748 | // ODEMETRIA X addr: l |
| ISR | 1:8569ac717e68 | 749 | |
| ISR | 1:8569ac717e68 | 750 | float X_od = X; //read value from device |
| ISR | 1:8569ac717e68 | 751 | |
| ISR | 1:8569ac717e68 | 752 | char X_od_R[sizeof(X_od)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 753 | char X_od_W[sizeof(X_od)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 754 | |
| ISR | 1:8569ac717e68 | 755 | snprintf(X_od_R, sizeof(X_od_R), "%.1f", X_od); //int value to char array |
| ISR | 1:8569ac717e68 | 756 | |
| ISR | 1:8569ac717e68 | 757 | for (int i=0; i < sizeof(X_od)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 758 | { |
| ISR | 1:8569ac717e68 | 759 | X_od_W[i+1]=X_od_R[i]; |
| ISR | 1:8569ac717e68 | 760 | } |
| ISR | 1:8569ac717e68 | 761 | |
| ISR | 1:8569ac717e68 | 762 | X_od_W[0]='l'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 763 | |
| ISR | 1:8569ac717e68 | 764 | |
| ISR | 1:8569ac717e68 | 765 | // ODEMETRIA X addr: m |
| ISR | 1:8569ac717e68 | 766 | |
| ISR | 1:8569ac717e68 | 767 | float Y_od = Y; //read value from device |
| ISR | 1:8569ac717e68 | 768 | |
| ISR | 1:8569ac717e68 | 769 | char Y_od_R[sizeof(Y_od)+1]; //char array ..._R to read value |
| ISR | 1:8569ac717e68 | 770 | char Y_od_W[sizeof(Y_od)+2]; //char array ..._W to write value |
| ISR | 1:8569ac717e68 | 771 | |
| ISR | 1:8569ac717e68 | 772 | snprintf(Y_od_R, sizeof(Y_od_R), "%.1f", Y_od); //int value to char array |
| ISR | 0:15a30802e719 | 773 | |
| ISR | 1:8569ac717e68 | 774 | for (int i=0; i < sizeof(Y_od)+2;i++) //changing positision of chars in array |
| ISR | 1:8569ac717e68 | 775 | { |
| ISR | 1:8569ac717e68 | 776 | Y_od_W[i+1]=Y_od_R[i]; |
| ISR | 1:8569ac717e68 | 777 | } |
| ISR | 1:8569ac717e68 | 778 | |
| ISR | 1:8569ac717e68 | 779 | Y_od_W[0]='m'; //adding id to parameter |
| ISR | 1:8569ac717e68 | 780 | |
| ISR | 1:8569ac717e68 | 781 | |
| ISR | 1:8569ac717e68 | 782 | |
| ISR | 1:8569ac717e68 | 783 | /////Preparing messages to send////// |
| ISR | 1:8569ac717e68 | 784 | |
| ISR | 1:8569ac717e68 | 785 | char send_Z[30]; |
| ISR | 1:8569ac717e68 | 786 | for (int i =0; i<sizeof(left_encoder_W);i++){ |
| ISR | 1:8569ac717e68 | 787 | send_Z[i]=left_encoder_W[i]; |
| ISR | 1:8569ac717e68 | 788 | } |
| ISR | 1:8569ac717e68 | 789 | strcat(send_Z,right_encoder_W); |
| ISR | 1:8569ac717e68 | 790 | strcat(send_Z,left_sensor_W); |
| ISR | 1:8569ac717e68 | 791 | strcat(send_Z,front_sensor_W); |
| ISR | 1:8569ac717e68 | 792 | strcat(send_Z,right_sensor_W); |
| ISR | 1:8569ac717e68 | 793 | strcat(send_Z,X_od_W); |
| ISR | 1:8569ac717e68 | 794 | |
| ISR | 1:8569ac717e68 | 795 | char send_Q[40]; |
| ISR | 1:8569ac717e68 | 796 | for (int i =0; i<sizeof(infrared_0_W);i++){ |
| ISR | 1:8569ac717e68 | 797 | send_Q[i]=infrared_0_W[i]; |
| ISR | 1:8569ac717e68 | 798 | } |
| ISR | 1:8569ac717e68 | 799 | strcat(send_Q,infrared_1_W); |
| ISR | 1:8569ac717e68 | 800 | strcat(send_Q,infrared_2_W); |
| ISR | 1:8569ac717e68 | 801 | strcat(send_Q,infrared_3_W); |
| ISR | 1:8569ac717e68 | 802 | strcat(send_Q,infrared_4_W); |
| ISR | 1:8569ac717e68 | 803 | strcat(send_Q,infrared_5_W); |
| ISR | 1:8569ac717e68 | 804 | strcat(send_Q,Y_od_W); |
| ISR | 1:8569ac717e68 | 805 | |
| ISR | 1:8569ac717e68 | 806 | |
| ISR | 1:8569ac717e68 | 807 | |
| ISR | 1:8569ac717e68 | 808 | /////Sending messages////// |
| ISR | 1:8569ac717e68 | 809 | radio.stopListening(); |
| ISR | 1:8569ac717e68 | 810 | radio.write( &send_Z, sizeof(send_Z)); |
| ISR | 1:8569ac717e68 | 811 | radio.write( &send_Q, sizeof(send_Q) ); |
| ISR | 1:8569ac717e68 | 812 | radio.startListening(); |
| ISR | 1:8569ac717e68 | 813 | } |
| ISR | 0:15a30802e719 | 814 | |
| ISR | 0:15a30802e719 | 815 | |
| ISR | 0:15a30802e719 | 816 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 817 | ////////////////////////////////// MISC. //////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 818 | /////////////////////////////////////////////////////////////////////////////////////////////// |
| ISR | 0:15a30802e719 | 819 | |
| ISR | 0:15a30802e719 | 820 | |
| ISR | 0:15a30802e719 | 821 | /** |
| ISR | 0:15a30802e719 | 822 | * Initializes the necessary robot pins |
| ISR | 0:15a30802e719 | 823 | */ |
| ISR | 0:15a30802e719 | 824 | void init_robot_pins() |
| ISR | 0:15a30802e719 | 825 | { |
| ISR | 0:15a30802e719 | 826 | |
| ISR | 0:15a30802e719 | 827 | //SAIDAS DIGITAIS (normal) |
| ISR | 0:15a30802e719 | 828 | //q_pha_mot_rig=0; //Phase Motor Right |
| ISR | 0:15a30802e719 | 829 | //q_sleep_mot_rig=0; //Nano Sleep Motor Right |
| ISR | 0:15a30802e719 | 830 | //q_pha_mot_lef=0; //Phase Motor Left |
| ISR | 0:15a30802e719 | 831 | //q_sleep_mot_lef=0; //Nano Sleep Motor Left |
| ISR | 0:15a30802e719 | 832 | //q_pow_ena_i2c_p=0; //Power Enable i2c FET P |
| ISR | 0:15a30802e719 | 833 | //q_pow_ena_mic_p=0; //Power enable Micro FET P |
| ISR | 0:15a30802e719 | 834 | //q_pow_as5600_n=1; //AS5600 Power MOSFET N |
| ISR | 0:15a30802e719 | 835 | //q_pow_as5600_p=0; //AS5600 Power MOSFET P |
| ISR | 0:15a30802e719 | 836 | //q_pow_spi=0; //SPI Power MOSFET P |
| ISR | 0:15a30802e719 | 837 | //q_ena_mppt=0; //Enable MPPT Control |
| ISR | 0:15a30802e719 | 838 | //q_boost_ps=1; //Boost Power Save |
| ISR | 0:15a30802e719 | 839 | //q_tca9548_reset=1; //Reset TCA9548 |
| ISR | 0:15a30802e719 | 840 | |
| ISR | 0:15a30802e719 | 841 | //SAIDAS DIGITAIS (normal) |
| ISR | 0:15a30802e719 | 842 | q_pha_mot_rig=0; //Phase Motor Right |
| ISR | 0:15a30802e719 | 843 | q_sleep_mot_rig=0; //Nano Sleep Motor Right |
| ISR | 0:15a30802e719 | 844 | q_pha_mot_lef=0; //Phase Motor Left |
| ISR | 0:15a30802e719 | 845 | q_sleep_mot_lef=0; //Nano Sleep Motor Left |
| ISR | 0:15a30802e719 | 846 | |
| ISR | 0:15a30802e719 | 847 | q_pow_ena_i2c_p=0; //Power Enable i2c FET P |
| ISR | 0:15a30802e719 | 848 | q_pow_ena_mic_p=0; //Power enable Micro FET P |
| ISR | 0:15a30802e719 | 849 | q_pow_as5600_p=0; //AS5600 Power MOSFET P |
| ISR | 0:15a30802e719 | 850 | // q_pow_spi=0; //SPI Power MOSFET P |
| ISR | 0:15a30802e719 | 851 | q_pow_as5600_n=1; //AS5600 Power MOSFET N |
| ISR | 0:15a30802e719 | 852 | |
| ISR | 0:15a30802e719 | 853 | |
| ISR | 0:15a30802e719 | 854 | q_ena_mppt=0; //Enable MPPT Control |
| ISR | 0:15a30802e719 | 855 | q_boost_ps=1; //Boost Power Save |
| ISR | 0:15a30802e719 | 856 | q_tca9548_reset=1; //Reset TCA9548 |
| ISR | 0:15a30802e719 | 857 | |
| ISR | 0:15a30802e719 | 858 | //Leds caso seja saida digital: |
| ISR | 0:15a30802e719 | 859 | q_led_red_fro=1; //Led Red Front (led off) |
| ISR | 0:15a30802e719 | 860 | q_led_gre_fro=1; //Led Green Front (led off) |
| ISR | 0:15a30802e719 | 861 | q_led_blu_fro=1; //Led Blue Front (led off) |
| ISR | 0:15a30802e719 | 862 | q_led_red_rea=1; //Led Red Rear (led off) |
| ISR | 0:15a30802e719 | 863 | q_led_gre_rea=1; //Led Green Rear (led off) |
| ISR | 0:15a30802e719 | 864 | q_led_blu_rea=1; //Led Blue Rear (led off)r |
| ISR | 0:15a30802e719 | 865 | |
| ISR | 0:15a30802e719 | 866 | |
| ISR | 0:15a30802e719 | 867 | //******************************************************************** |
| ISR | 0:15a30802e719 | 868 | //SAIDAS DIGITAIS (pwm) |
| ISR | 0:15a30802e719 | 869 | //PWM Enable Motor Right |
| ISR | 0:15a30802e719 | 870 | pwm_mot_rig.period_us(500); |
| ISR | 0:15a30802e719 | 871 | pwm_mot_rig.pulsewidth_us(0); |
| ISR | 0:15a30802e719 | 872 | |
| ISR | 0:15a30802e719 | 873 | //PWM Enable Motor Left |
| ISR | 0:15a30802e719 | 874 | pwm_mot_lef.period_us(500); |
| ISR | 0:15a30802e719 | 875 | pwm_mot_lef.pulsewidth_us(0); |
| ISR | 0:15a30802e719 | 876 | |
| ISR | 0:15a30802e719 | 877 | //Buzzer PWM |
| ISR | 0:15a30802e719 | 878 | pwm_buzzer.period_us(500); |
| ISR | 0:15a30802e719 | 879 | pwm_buzzer.pulsewidth_us(0); |
| ISR | 0:15a30802e719 | 880 | |
| ISR | 1:8569ac717e68 | 881 | |
| ISR | 0:15a30802e719 | 882 | //LED white |
| ISR | 0:15a30802e719 | 883 | pwm_led_whi.period_us(500); |
| ISR | 0:15a30802e719 | 884 | pwm_led_whi.pulsewidth_us(0); |
| ISR | 0:15a30802e719 | 885 | |
| ISR | 0:15a30802e719 | 886 | } |
| ISR | 0:15a30802e719 | 887 | |
| ISR | 0:15a30802e719 | 888 | /** |
| ISR | 0:15a30802e719 | 889 | * Initializes all the pins and all the modules necessary |
| ISR | 1:8569ac717e68 | 890 | * |
| ISR | 1:8569ac717e68 | 891 | * @param channel - Which RF channel to comunicate on, 0-125. |
| ISR | 1:8569ac717e68 | 892 | *\note If you are not using RF module put random number. |
| ISR | 1:8569ac717e68 | 893 | * \warning Channel on Robot and Board has to be the same. |
| ISR | 0:15a30802e719 | 894 | */ |
| ISR | 1:8569ac717e68 | 895 | void initRobot(int channel) |
| ISR | 1:8569ac717e68 | 896 | { |
| ISR | 1:8569ac717e68 | 897 | pc.printf("Battery level: \n\r"); |
| ISR | 1:8569ac717e68 | 898 | init_nRF(channel); |
| ISR | 0:15a30802e719 | 899 | init_robot_pins(); |
| ISR | 0:15a30802e719 | 900 | enable_dc_dc_boost(); |
| ISR | 1:8569ac717e68 | 901 | wait_ms(100); //wait for read wait(>=150ms); |
| ISR | 0:15a30802e719 | 902 | enable_dc_dc_boost(); |
| ISR | 1:8569ac717e68 | 903 | measure_always_on(); |
| ISR | 0:15a30802e719 | 904 | wait_ms(100); //wait for read wait(>=150ms); |
| ISR | 1:8569ac717e68 | 905 | |
| ISR | 1:8569ac717e68 | 906 | init_Infrared(); |
| ISR | 0:15a30802e719 | 907 | float value = value_of_Batery(); |
| ISR | 1:8569ac717e68 | 908 | |
| ISR | 1:8569ac717e68 | 909 | thread.start(odometry_thread); |
| ISR | 1:8569ac717e68 | 910 | |
| ISR | 0:15a30802e719 | 911 | pc.printf("Initialization Successful \n\r"); |
| ISR | 0:15a30802e719 | 912 | pc.printf("Battery level: %f \n\r",value); |
| ISR | 0:15a30802e719 | 913 | if(value < 3.0) { |
| ISR | 0:15a30802e719 | 914 | pc.printf(" WARNING: BATTERY NEEDS CHARGING "); |
| ISR | 0:15a30802e719 | 915 | } |
| ISR | 0:15a30802e719 | 916 | // float level = value_of_Batery(); |
| ISR | 0:15a30802e719 | 917 | // sendValue(int(level*100)); |
| ISR | 0:15a30802e719 | 918 | |
| ISR | 0:15a30802e719 | 919 | } |