ISR UC / ISR_Mini-explorer

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

Dependencies:   RF24

Dependents:   Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more

robot.h@2:0435d1171673, 2018-02-20 (annotated)

Committer:
ISR
Date:
Tue Feb 20 17:17:41 2018 +0000
Revision:
2:0435d1171673
Parent:
1:8569ac717e68
Child:
3:215c9544480d
Global theta added

Who changed what in which revision?

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