ISR UC / ISR_Mini-explorer

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

Dependents:   Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more

Committer:
ISR
Date:
Thu Feb 02 12:21:11 2017 +0000
Revision:
0:15a30802e719
Initial commit.

Who changed what in which revision?

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