ISR UC
Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Dependents: Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more
robot.h
- Committer:
- ISR
- Date:
- 2018-02-20
- Revision:
- 1:8569ac717e68
- Parent:
- 0:15a30802e719
- Child:
- 2:0435d1171673
File content as of revision 1:8569ac717e68:
/** @file */
#include <math.h>
#include <string.h>
#include "VCNL40x0.h"
#include <RF24.h>
#include "Encoder.h"
I2C i2c(PTC9,PTC8);
I2C i2c1(PTC11,PTC10);
float X=20;
float Y=20;
Mutex mutex_i2c0, mutex_i2c1;
Encoder esquerdo(&i2c, &mutex_i2c0, 0);
Encoder direito(&i2c1, &mutex_i2c1, 1);
Thread thread;
void odometry_thread()
{
float theta=0;
long int ticks2d=0;
long int ticks2e=0;
while (true) {
esquerdo.incremental();
direito.incremental();
//-------------------------------------------
long int ticks1d=direito.readIncrementalValue();
long int ticks1e=esquerdo.readIncrementalValue();
long int D_ticks=ticks1d - ticks2d;
long int E_ticks=ticks1e - ticks2e;
ticks2d=ticks1d;
ticks2e=ticks1e;
float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
float CM=(D_cm + L_cm)/2;
theta +=(D_cm - L_cm)/7.18;
theta = atan2(sin(theta), cos(theta));
// meter entre 0
X += CM*cos(theta);
Y += CM*sin(theta);
//-------------------------------------------
Thread::wait(10);
}
}
// classes adicionais
RF24 radio(PTD2, PTD3, PTD1, PTC12 ,PTC13);
VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
Timeout timeout;
Serial pc(PTE0,PTE1);
// Variables needed by the lib
unsigned int ProxiValue=0;
//SAIDAS DIGITAIS (normal)
DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right
DigitalOut q_sleep_mot_rig (PTE3,0); //Nano Sleep Motor Right
DigitalOut q_pha_mot_lef (PTA17,0); //Phase Motor Left
DigitalOut q_sleep_mot_lef (PTB11,0); //Nano Sleep Motor Left
DigitalOut q_pow_ena_i2c_p (PTE2,0); //Power Enable i2c FET P (0- enable 1-disable)
DigitalOut q_pow_ena_mic_p (PTA14,0); //Power enable Micro FET P (0- enable 1-disable)
DigitalOut q_pow_as5600_n (PTC6,1); //AS5600 Power MOSFET N (1- enable 0-disable)
DigitalOut q_pow_as5600_p (PTC5,0); //AS5600 Power MOSFET P (0- enable 1-disable)
DigitalOut q_pow_spi (PTC4,0); //SPI Power MOSFET P (0- enable 1-disable)
DigitalOut q_ena_mppt (PTC0,0); //Enable MPPT Control (0- enable 1-disable)
DigitalOut q_boost_ps (PTC7,1); //Boost Power Save (1- enable 0-disable)
DigitalOut q_tca9548_reset (PTC3,1); //Reset TCA9548 (1- enable 0-disable)
DigitalOut power_36khz (PTD0,0); //Power enable pic12f - 36khz (0- enable 1-disable)
// ********************************************************************
// ********************************************************************
//DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
//ENTRADAS DIGITAIS (normal input)
DigitalIn i_enc_dir_rig (PTB8); //Encoder Right Direction
DigitalIn i_enc_dir_lef (PTB9); //Encoder Left Direction
DigitalIn i_micro_sd_det (PTC16); //MICRO SD Card Detect
DigitalIn i_mppt_fail (PTE5); //Fail MPPT Signal
DigitalIn i_usb_volt (PTB10); //USB Voltage detect
DigitalIn i_sup_cap_est (PTB19); //Supercap State Charger
DigitalIn i_li_ion_est (PTB18); //Li-ion State Charger
// ********************************************************************
//ENTRADAS DIGITAIS (external interrupt)
InterruptIn i_int_mpu9250 (PTA15); //Interrupt MPU9250
InterruptIn i_int_isl29125 (PTA16); //Interrupt ISL29125 Color S.
InterruptIn i_mic_f_l (PTD7); //Interrupt Comp Micro F L
InterruptIn i_mic_f_r (PTD6); //Interrupt Comp Micro F R
InterruptIn i_mic_r_c (PTD5); //Interrupt Comp Micro R C
// ********************************************************************
//ENTRADAS ANALOGICAS
AnalogIn a_enc_rig (PTC2); //Encoder Left Output_AS_MR
AnalogIn a_enc_lef (PTC1); //Encoder Right Output_AS_ML
AnalogIn a_mic_f_l (PTB0); //Analog microphone F L
AnalogIn a_mic_f_r (PTB1); //Analog microphone F R
AnalogIn a_mic_r_c (PTB2); //Analog microphone R C
AnalogIn a_temp_bat (PTB3); //Temperature Battery
// ********************************************************************
//PWM OR DIGITAL OUTPUT NORMAL
//DigitalOut q_led_whi (PTE29); //Led white pwm
DigitalOut q_led_red_fro (PTA4); //Led Red Front
DigitalOut q_led_gre_fro (PTA5); //Led Green Front
DigitalOut q_led_blu_fro (PTA12); //Led Blue Front
DigitalOut q_led_red_rea (PTD4); //Led Red Rear
DigitalOut q_led_gre_rea (PTA1); //Led Green Rear
DigitalOut q_led_blu_rea (PTA2); //Led Blue Rear
//SAIDAS DIGITAIS (pwm)
PwmOut pwm_mot_rig (PTE20); //PWM Enable Motor Right
PwmOut pwm_mot_lef (PTE31); //PWM Enable Motor Left
PwmOut pwm_buzzer (PTE21); //Buzzer PWM
PwmOut pwm_led_whi (PTE29); //Led white pwm
// ********************************************************************
//SAIDAS ANALOGICAS
AnalogOut dac_comp_mic (PTE30); //Dac_Comparator MIC
/**
* Selects the wich infrared to comunicate.
*
* @param ch - Infrared to read (1..5)
*/
void tca9548_select_ch(char ch)
{
char ch_f[1];
char addr=0xE0;
if(ch==0)
ch_f[0]=1;
if(ch>=1)
ch_f[0]=1<<ch;
mutex_i2c0.lock();
i2c.write(addr,ch_f,1);
mutex_i2c0.unlock();
}
/* Powers up all the VCNL4020. */
void init_Infrared()
{
for (int i=0; i<6;i++)
{
tca9548_select_ch(i);
VCNL40x0_Device.SetCurrent (20); // Set current to 200mA
}
tca9548_select_ch(0);
}
/**
* Get the distance from of the chosen infrared.
*
* @param ch - Infrared to read (0,1..5)
*
* Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
*/
float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
{
tca9548_select_ch(ch);
VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue); // read prox value on demand
float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
return aux;
//return ProxiValue;
}
///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// MOTOR ///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
// Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
// consultar pag 39 e 95
/**
* Sets speed and direction of the left motor.
*
* @param Dir - Direction of movement, 0 for back, or 1 for fron
* @param Speed - Percentage of speed of the motor (1..100)
*
* Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
* at different speeds and see if it makes a straigth line
*/
void leftMotor(short int Dir,short int Speed)
{
float Duty;
if(Dir==1) {
q_pha_mot_lef=0; //Andar em frente
if(Speed>1000) //limite de segurança
Speed=1000;
if(Speed>0) {
Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
q_sleep_mot_lef=1; //Nano Sleep Motor Left
pwm_mot_lef.pulsewidth_us(Duty*5);
} else {
q_sleep_mot_lef=0;
}
}
if(Dir==0) {
q_pha_mot_lef=1; //Andar para tras
if(Speed>1000) //limite de segurança
Speed=1000;
if(Speed>0) {
Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
q_sleep_mot_lef=1; //Nano Sleep Motor Left
pwm_mot_lef.pulsewidth_us(Duty*5);
} else {
q_sleep_mot_lef=0;
}
}
}
/**
* Sets speed and direction of the right motor.
*
* @param Dir - Direction of movement, 0 for back, or 1 for fron
* @param Speed - Percentage of speed of the motor (1..100)
*
* Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
* at different speeds and see if it makes a straigth line
*/
void rightMotor(short int Dir,short int Speed)
{
float Duty;
if(Dir==1) {
q_pha_mot_rig=0; //Andar em frente
if(Speed>1000) //limite de segurança
Speed=1000;
if(Speed>0) {
Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
q_sleep_mot_rig=1; //Nano Sleep Motor Right
pwm_mot_rig.pulsewidth_us(Duty*5);
} else {
q_sleep_mot_rig=0;
}
}
if(Dir==0) {
q_pha_mot_rig=1; //Andar para tras
if(Speed>1000) //limite de segurança
Speed=1000;
if(Speed>0) {
Duty=Speed*.082 +35; // 35 = minimo para o motor rodar
q_sleep_mot_rig=1; //Nano Sleep Motor Right
pwm_mot_rig.pulsewidth_us(Duty*5);
} else {
q_sleep_mot_rig=0;
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// BATTERY ///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/**
* Reads adc of the battery.
*
* @param addr - Address to read
* @return The voltage of the baterry
*/
long int read16_mcp3424(char addr)
{
char data[4];
mutex_i2c1.lock();
i2c1.read(addr,data,3);
mutex_i2c1.unlock();
return(((data[0]&127)*256)+data[1]);
}
/**
* Reads adc of the battery.
*
* @param n_bits - Resolution of measure
* @param ch - Chose value to read, if voltage or current of solar or batery
* @param gain -
* @param addr - Address to write to
*/
void write_mcp3424(int n_bits, int ch, int gain, char addr) //chanel 1-4 write -> 0xD0
{
int chanel_end=(ch-1)<<5; //shift left
char n_bits_end=0;
if(n_bits==12) {
n_bits_end=0;
} else if(n_bits==14) {
n_bits_end=1;
} else if(n_bits==16) {
n_bits_end=2;
} else {
n_bits_end=3;
}
n_bits_end=n_bits_end<<2; //shift left
char data[1];
data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
mutex_i2c1.lock();
i2c1.write(addr,data,1);
mutex_i2c1.unlock();
}
/**
* Reads adc of the battery.
*
* @return The voltage of the batery
*/
float value_of_Batery()
{
float R1=75000.0;
float R2=39200.0;
float R3=178000.0;
float Gain=1.0;
write_mcp3424(16,3,1,0xd8);
float cha3_v2=read16_mcp3424(0xd9); //read voltage
float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
return Vin_b_v_battery;
}
///////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// Sonar ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
// Commands of operation with ultrasonic module
// WRITE OPTION:
// ENABLE DC DC CONVERTER - 0x0C;
// DISABLE DC DC CONVERTER - 0x0B;
// START MEASURE LEFT SENSOR - 0x0A;
// START MEASURE FRONT SENSOR - 0x09;
// START MEASURE RIGHT SENSOR - 0x08;
// SENSORS ALWAYS MEASURE ON - 0x07;
// SENSORS ALWAYS MEASURE OFF - 0x06;
// READ OPTION:
// GET MEASURE OF LEFT SENSOR - 0x05;
// GET MEASURE OF FRONT SENSOR - 0x04;
// GET MEASURE OF IGHT SENSOR - 0x03;
// GET STATUS SENSORS ALWAYS MEASURE - 0x02;
// GET STATUS DC DC CONVERTER - 0x01;
void enable_dc_dc_boost()
{
char data[1];
data[0]= 0x0C;
mutex_i2c1.lock();
i2c1.write(0x30,data,1);
mutex_i2c1.unlock();
}
void measure_always_on() // left, front, right
{
char data[1];
data[0]= 0x07;
mutex_i2c1.lock();
i2c1.write(0x30,data,1);
mutex_i2c1.unlock();
}
/**
* Returns left sensor value
*/
static unsigned int get_distance_left_sensor()
{
static char data_r[3];
static unsigned int aux;
data_r[0]= 0x05;
mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
i2c1.read(0x31,data_r,2);
mutex_i2c1.unlock();
aux=(data_r[0]*256)+data_r[1];
return aux;
}
/**
* Returns front sensor value
*/
static unsigned int get_distance_front_sensor()
{
static char data_r[3];
static unsigned int aux;
data_r[0]= 0x04;
mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
i2c1.read(0x31,data_r,2);
mutex_i2c1.unlock();
aux=(data_r[0]*256)+data_r[1];
return aux;
}
/**
* Returns right sensor value
*/
static unsigned int get_distance_right_sensor()
{
static char data_r[3];
static unsigned int aux;
data_r[0]= 0x03;
mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
i2c1.read(0x31,data_r,2,0);
mutex_i2c1.unlock();
aux=(data_r[0]*256)+data_r[1];
return aux;
}
///////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// RF COMMUNICATION ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes nrf24l01 module, return value 1 if module is working correcty and 0 if it's not.
*
** @param channel - Which RF channel to comunicate on, 0-125
*
* \warning Channel on Robot and Board has to be the same.
*/
void init_nRF(int channel){
int result;
result = radio.begin();
pc.printf( "Initialation nrf24l01=%d\r\n", result ); // 1-working,0-not working
radio.setDataRate(RF24_1MBPS);
radio.setCRCLength(RF24_CRC_16);
radio.setPayloadSize(32);
radio.setChannel(channel);
radio.setAutoAck(true);
radio.openWritingPipe(0x314e6f6465);
radio.openReadingPipe(1,0x324e6f6465);
radio.startListening();
}
char txData, rxData;
char q[1];
/**
* Wireless control of motors using nrf24l01 module and FRDM-KL25Z board(robot is working as receiver).
*/
void robot_RC()
{
if (radio.available() ) {
radio.read( &rxData, sizeof(rxData) );
pc.putc( rxData );
q[0]=pc.putc( rxData );
if (q[0]=='w'){
leftMotor(1,500);
rightMotor(1,500);
}
else if( q[0]=='s'){
leftMotor(0,500);
rightMotor(0,500);
}
else if( q[0]=='a'){
leftMotor(0,500);
rightMotor(1,500);
}
else if( q[0]=='d'){
leftMotor(1,500);
rightMotor(0,1000);
}
else if( q[0]==' '){
leftMotor(1,0);
rightMotor(1,0);
}
}
}
/**
* Robot is sending values of all sensors (right/left incremental encoder, ultrasonic sensors, infarated sensors and Odometria) to FRDM-KL25Z board using nrf24l01 module.
*
*Note: Check if module is initilized correctly.
*/
void send_Robot_values(){
// WRITE ID:
// INCREMENTAL LEFT ENCODER - a
// INCREMENTAL RIGHT ENCODER - b
// LEFT ULTRASENSOR - c
// FRONT ULTRASENSOR - d
// RIGHT ULTRASENSOR - e
// INFRARED SENSOR 0 - f
// INFRARED SENSOR 1 - g
// INFRARED SENSOR 2 - h
// INFRARED SENSOR 3 - i
// INFRARED SENSOR 4 - j
// INFRARED SENSOR 5 - k
// ODOMETRIA "X" - l
// ODOMETRIA "Y" - m
// LEFT ENCODER addr: a
int left_encoder = 0; //read value from device
char left_encoder_R[10]; //char array ..._R to read value
char left_encoder_W[12]; //char array ..._W to write value
snprintf(left_encoder_R, 10, "%d", left_encoder); //int value to char array
for (int i=0; i < 10;i++) //changing posiision of chars in array
{
left_encoder_W[i+2]=left_encoder_R[i];
}
left_encoder_W[0]='Q'; //adding id to message
left_encoder_W[1]='a'; //adding id to parameter
// RIGHT ENCODER addr: b
int right_encoder = 0; //read value from device
char right_encoder_R[10]; //char array ..._R to read value
char right_encoder_W[11]; //char array ..._W to write value
snprintf(right_encoder_R, 10, "%d", right_encoder); //int value to char array
for (int i=0; i <10;i++) //changing positision of chars in array
{
right_encoder_W[i+1]=right_encoder_R[i];
}
right_encoder_W[0]='b'; //adding id to parameter
// LEFT ULTRASENSOR addr: c
int left_sensor = get_distance_left_sensor(); //read value from device
char left_sensor_R[5]; //char array ..._R to read value
char left_sensor_W[6]; //char array ..._W to write value
snprintf(left_sensor_R, 5, "%d", left_sensor); //int value to char array
for (int i=0; i < 5;i++) //changing positision of chars in array
{
left_sensor_W[i+1]=left_sensor_R[i];
}
left_sensor_W[0]='c'; //adding id to parameter
// FRONT ULTRASENSOR addr: d
int front_sensor = get_distance_front_sensor(); //read value from device
char front_sensor_R[5]; //char array ..._R to read value
char front_sensor_W[6]; //char array ..._W to write value
snprintf(front_sensor_R, 5, "%d", front_sensor); //int value to char array
for (int i=0; i < 5;i++) //changing positision of chars in array
{
front_sensor_W[i+1]=front_sensor_R[i];
}
front_sensor_W[0]='d'; //adding id to parameter
// RIGHT ULTRASENSOR addr: e
int right_sensor = get_distance_right_sensor(); //read value from device
char right_sensor_R[5]; //char array ..._R to read value
char right_sensor_W[6]; //char array ..._W to write value
snprintf(right_sensor_R, 5, "%d", right_sensor); //int value to char array
for (int i=0; i < 5;i++) //changing positision of chars in array
{
right_sensor_W[i+1]=right_sensor_R[i];
}
right_sensor_W[0]='e'; //adding id to parameter
// INFRARED SENSOR 0 addr: f
float infrared_0 = read_Infrared(0); //read value from device
char infrared_0_R[5]; //char array ..._R to read value
char infrared_0_W[7]; //char array ..._W to write value
snprintf(infrared_0_R, 5, "%.1f", infrared_0); //int value to char array
for (int i=0; i < 6;i++) //changing positision of chars in array
{
infrared_0_W[i+2]=infrared_0_R[i];
}
infrared_0_W[0]='Z'; //adding id to message
infrared_0_W[1]='f'; //adding id to parameter
// INFRARED SENSOR 1 addr: g
float infrared_1 = read_Infrared(1); //read value from device
char infrared_1_R[sizeof(infrared_1)+1]; //char array ..._R to read value
char infrared_1_W[sizeof(infrared_1)+2]; //char array ..._W to write value
snprintf(infrared_1_R, sizeof(infrared_1_R), "%.1f", infrared_1); //int value to char array
for (int i=0; i < sizeof(infrared_1)+2;i++) //changing positision of chars in array
{
infrared_1_W[i+1]=infrared_1_R[i];
}
infrared_1_W[0]='g'; //adding id to parameter
// INFRARED SENSOR 2 addr: h
float infrared_2 = read_Infrared(2); //read value from device
char infrared_2_R[sizeof(infrared_2)+1]; //char array ..._R to read value
char infrared_2_W[sizeof(infrared_2)+2]; //char array ..._W to write value
snprintf(infrared_2_R, sizeof(infrared_2_R), "%.1f", infrared_2); //int value to char array
for (int i=0; i < sizeof(infrared_2)+2;i++) //changing positision of chars in array
{
infrared_2_W[i+1]=infrared_2_R[i];
}
infrared_2_W[0]='h'; //adding id to parameter
// INFRARED SENSOR 3 addr: i
float infrared_3 = read_Infrared(3); //read value from device
char infrared_3_R[sizeof(infrared_3)+1]; //char array ..._R to read value
char infrared_3_W[sizeof(infrared_3)+2]; //char array ..._W to write value
snprintf(infrared_3_R, sizeof(infrared_3_R), "%.1f", infrared_3); //int value to char array
for (int i=0; i < sizeof(infrared_3)+2;i++) //changing positision of chars in array
{
infrared_3_W[i+1]=infrared_3_R[i];
}
infrared_3_W[0]='i'; //adding id to parameter
// INFRARED SENSOR 4 addr: j
float infrared_4 = read_Infrared(4); //read value from device
char infrared_4_R[sizeof(infrared_4)+1]; //char array ..._R to read value
char infrared_4_W[sizeof(infrared_4)+2]; //char array ..._W to write value
snprintf(infrared_4_R, sizeof(infrared_4_R), "%.1f", infrared_4); //int value to char array
for (int i=0; i < sizeof(infrared_4)+2;i++) //changing positision of chars in array
{
infrared_4_W[i+1]=infrared_4_R[i];
}
infrared_4_W[0]='j'; //adding id to parameter
// INFRARED SENSOR 5 addr: k
float infrared_5 = read_Infrared(5); //read value from device
char infrared_5_R[sizeof(infrared_5)+1]; //char array ..._R to read value
char infrared_5_W[sizeof(infrared_5)+2]; //char array ..._W to write value
snprintf(infrared_5_R, sizeof(infrared_5_R), "%.1f", infrared_5); //int value to char array
for (int i=0; i < sizeof(infrared_5)+2;i++) //changing positision of chars in array
{
infrared_5_W[i+1]=infrared_5_R[i];
}
infrared_5_W[0]='k'; //adding id to parameter
// ODEMETRIA X addr: l
float X_od = X; //read value from device
char X_od_R[sizeof(X_od)+1]; //char array ..._R to read value
char X_od_W[sizeof(X_od)+2]; //char array ..._W to write value
snprintf(X_od_R, sizeof(X_od_R), "%.1f", X_od); //int value to char array
for (int i=0; i < sizeof(X_od)+2;i++) //changing positision of chars in array
{
X_od_W[i+1]=X_od_R[i];
}
X_od_W[0]='l'; //adding id to parameter
// ODEMETRIA X addr: m
float Y_od = Y; //read value from device
char Y_od_R[sizeof(Y_od)+1]; //char array ..._R to read value
char Y_od_W[sizeof(Y_od)+2]; //char array ..._W to write value
snprintf(Y_od_R, sizeof(Y_od_R), "%.1f", Y_od); //int value to char array
for (int i=0; i < sizeof(Y_od)+2;i++) //changing positision of chars in array
{
Y_od_W[i+1]=Y_od_R[i];
}
Y_od_W[0]='m'; //adding id to parameter
/////Preparing messages to send//////
char send_Z[30];
for (int i =0; i<sizeof(left_encoder_W);i++){
send_Z[i]=left_encoder_W[i];
}
strcat(send_Z,right_encoder_W);
strcat(send_Z,left_sensor_W);
strcat(send_Z,front_sensor_W);
strcat(send_Z,right_sensor_W);
strcat(send_Z,X_od_W);
char send_Q[40];
for (int i =0; i<sizeof(infrared_0_W);i++){
send_Q[i]=infrared_0_W[i];
}
strcat(send_Q,infrared_1_W);
strcat(send_Q,infrared_2_W);
strcat(send_Q,infrared_3_W);
strcat(send_Q,infrared_4_W);
strcat(send_Q,infrared_5_W);
strcat(send_Q,Y_od_W);
/////Sending messages//////
radio.stopListening();
radio.write( &send_Z, sizeof(send_Z));
radio.write( &send_Q, sizeof(send_Q) );
radio.startListening();
}
///////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// MISC. ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes the necessary robot pins
*/
void init_robot_pins()
{
//SAIDAS DIGITAIS (normal)
//q_pha_mot_rig=0; //Phase Motor Right
//q_sleep_mot_rig=0; //Nano Sleep Motor Right
//q_pha_mot_lef=0; //Phase Motor Left
//q_sleep_mot_lef=0; //Nano Sleep Motor Left
//q_pow_ena_i2c_p=0; //Power Enable i2c FET P
//q_pow_ena_mic_p=0; //Power enable Micro FET P
//q_pow_as5600_n=1; //AS5600 Power MOSFET N
//q_pow_as5600_p=0; //AS5600 Power MOSFET P
//q_pow_spi=0; //SPI Power MOSFET P
//q_ena_mppt=0; //Enable MPPT Control
//q_boost_ps=1; //Boost Power Save
//q_tca9548_reset=1; //Reset TCA9548
//SAIDAS DIGITAIS (normal)
q_pha_mot_rig=0; //Phase Motor Right
q_sleep_mot_rig=0; //Nano Sleep Motor Right
q_pha_mot_lef=0; //Phase Motor Left
q_sleep_mot_lef=0; //Nano Sleep Motor Left
q_pow_ena_i2c_p=0; //Power Enable i2c FET P
q_pow_ena_mic_p=0; //Power enable Micro FET P
q_pow_as5600_p=0; //AS5600 Power MOSFET P
// q_pow_spi=0; //SPI Power MOSFET P
q_pow_as5600_n=1; //AS5600 Power MOSFET N
q_ena_mppt=0; //Enable MPPT Control
q_boost_ps=1; //Boost Power Save
q_tca9548_reset=1; //Reset TCA9548
//Leds caso seja saida digital:
q_led_red_fro=1; //Led Red Front (led off)
q_led_gre_fro=1; //Led Green Front (led off)
q_led_blu_fro=1; //Led Blue Front (led off)
q_led_red_rea=1; //Led Red Rear (led off)
q_led_gre_rea=1; //Led Green Rear (led off)
q_led_blu_rea=1; //Led Blue Rear (led off)r
//********************************************************************
//SAIDAS DIGITAIS (pwm)
//PWM Enable Motor Right
pwm_mot_rig.period_us(500);
pwm_mot_rig.pulsewidth_us(0);
//PWM Enable Motor Left
pwm_mot_lef.period_us(500);
pwm_mot_lef.pulsewidth_us(0);
//Buzzer PWM
pwm_buzzer.period_us(500);
pwm_buzzer.pulsewidth_us(0);
//LED white
pwm_led_whi.period_us(500);
pwm_led_whi.pulsewidth_us(0);
}
/**
* Initializes all the pins and all the modules necessary
*
* @param channel - Which RF channel to comunicate on, 0-125.
*\note If you are not using RF module put random number.
* \warning Channel on Robot and Board has to be the same.
*/
void initRobot(int channel)
{
pc.printf("Battery level: \n\r");
init_nRF(channel);
init_robot_pins();
enable_dc_dc_boost();
wait_ms(100); //wait for read wait(>=150ms);
enable_dc_dc_boost();
measure_always_on();
wait_ms(100); //wait for read wait(>=150ms);
init_Infrared();
float value = value_of_Batery();
thread.start(odometry_thread);
pc.printf("Initialization Successful \n\r");
pc.printf("Battery level: %f \n\r",value);
if(value < 3.0) {
pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
}
// float level = value_of_Batery();
// sendValue(int(level*100));
}