Fabio Faria
Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Fork of
ISR_Mini-explorer
by 
ISR UC
Diff: robot.h
- Revision:
- 1:8569ac717e68
- Parent:
- 0:15a30802e719
- Child:
- 2:0435d1171673
--- a/robot.h Thu Feb 02 12:21:11 2017 +0000
+++ b/robot.h Tue Feb 20 13:37:47 2018 +0000
@@ -1,54 +1,79 @@
/** @file */
-//AUTOR: Fernando Pais
-//MAIL: ferpais2508@gmail.com
-//DATA: 6/6/2016
-// VERSÃO 6.4.0
-//
-//Alterações: Problema de compatibilidade entre encoder e infravermelho resolvido
-// Odometria actualizada automaticamente
-// Valor da bateria verificado na inicialização
-// Motores movem-se de 0 a 1000 para melhor difrenciação
-//
-//#include "mbed.h"
-//#include "init.h"
-//#define _USE_MATH_DEFINES
-# define M_PI 3.14159265358979323846 /* pi */
#include <math.h>
#include <string.h>
#include "VCNL40x0.h"
-#include "nRF24L01P.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;
+
+
-void Odometria();
+ 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
-nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
+RF24 radio(PTD2, PTD3, PTD1, PTC12 ,PTC13);
VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
+
+
Timeout timeout;
Serial pc(PTE0,PTE1);
-I2C i2c(PTC9,PTC8);
-I2C i2c1(PTC11,PTC10);
+
// Variables needed by the lib
unsigned int ProxiValue=0;
-short int prev_R=0;
-short int prev_L=0;
-long int total_R=0;
-long int total_L=0;
-long int ticks2d=0;
-long int ticks2e=0;
-float X=20;
-float Y=20;
-float AtractX = 0;
-float AtractY = 0;
-float theta=0;
-int sensor_left=0;
-int sensor_front=0;
-int sensor_right=0;
-short int flag=0;
-int IRobot=0;
-int JRobot=0;
//SAIDAS DIGITAIS (normal)
DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right
@@ -121,11 +146,7 @@
AnalogOut dac_comp_mic (PTE30); //Dac_Comparator MIC
-/* Powers up all the VCNL4020. */
-void init_Infrared()
-{
- VCNL40x0_Device.SetCurrent (20); // Set current to 200mA
-}
+
/**
* Selects the wich infrared to comunicate.
@@ -143,25 +164,42 @@
if(ch>=1)
ch_f[0]=1<<ch;
- i2c.start();
+mutex_i2c0.lock();
i2c.write(addr,ch_f,1);
- i2c.stop();
+ 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 ADC value of the chosen infrared.
+ * Get the distance from of the chosen infrared.
*
- * @param ch - Infrared to read (1..5)
+ * @param ch - Infrared to read (0,1..5)
*
* Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
*/
-long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
+float read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
{
tca9548_select_ch(ch);
VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue); // read prox value on demand
-
- return ProxiValue;
+ float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
+ return aux;
+ //return ProxiValue;
}
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -256,168 +294,6 @@
///////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////// ENCODER ///////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-/**
- * Reads Position of left magnetic encoder.
- *
- * @return The absolute position of the left wheel encoder (0..4095)
- */
-long int read_L_encoder()
-{
-
- char data_r_2[5];
-
- i2c.start();
- i2c.write(0x6C);
- i2c.write(0x0C);
- i2c.read(0x6D,data_r_2,4,0);
- i2c.stop();
-
- short int val1=data_r_2[0];
- short int val2=data_r_2[1];
- val1=(val1&0xf)*256;
- long int final=(val2+val1);
-
- return final;
-}
-
-
-/**
- * Reads Position of right magnetic encoder.
- *
- * @return The absolute position of the right wheel encoder (0..4095)
- */
-long int read_R_encoder()
-{
-
- char data_r_2[5];
-
- i2c1.start();
- i2c1.write(0x6C);
- i2c1.write(0x0C);
- i2c1.read(0x6D,data_r_2,4,0);
- i2c1.stop();
-
- short int val1=data_r_2[0];
- short int val2=data_r_2[1];
- val1=(val1&0xf)*256;
- long int final=(val2+val1);
-
- return final;
-}
-
-
-/**
- * Calculates and returns the value of the right "incremental" encoder.
- *
- * @return The value of "tics" of the right encoder since it was initialized
- */
-long int incremental_R_encoder()
-{
- short int next_R=read_R_encoder(); // Reads curent value of the encoder
- short int dif=next_R-prev_R; // Calculates the diference from last reading
-
- if(dif>3000) { // Going back and pass zero
- total_R=total_R-4096+dif;
- }
- if(dif<3000&&dif>0) { // Going front
- total_R=total_R+dif;
- }
- if(dif<-3000) { // Going front and pass zero
- total_R=total_R+4096+dif;
- }
- if(dif>-3000&&dif<0) { // going back
- total_R=total_R+dif;
- }
- prev_R=next_R; // Sets last reading for next iteration
-
- return total_R;
-}
-
-
-/**
- * Calculates and returns the value of the left "incremental" encoder.
- *
- * @return The value of "tics" of the left encoder since it was initialized
- */
-long int incremental_L_encoder()
-{
- short int next_L=read_L_encoder(); // Reads curent value of the encoder
- short int dif=-next_L+prev_L; // Calculates the diference from last reading
-
- if(dif>3000) { // Going back and pass zero
- total_L=total_L-4096+dif;
- }
- if(dif<3000&&dif>0) { // Going front
- total_L=total_L+dif;
- }
- if(dif<-3000) { // Going front and pass zero
- total_L=total_L+4096+dif;
- }
- if(dif>-3000&&dif<0) { // going back
- total_L=total_L+dif;
- }
- prev_L=next_L; // Sets last reading for next iteration
-
- return total_L;
-}
-
-
-/**
- * Calculate the value of both encoder "incremental" every 10 ms.
- */
-void timer_event() //10ms interrupt
-{
- timeout.attach(&timer_event,0.01);
- if(flag==0) {
- incremental_R_encoder();
- incremental_L_encoder();
- }
- Odometria();
-
- return;
-}
-
-
-/**
- * Set the initial position for the "incremental" enconder and "starts" them.
- */
-void initEncoders()
-{
- prev_R=read_R_encoder();
- prev_L=read_L_encoder();
- timeout.attach(&timer_event,0.01);
-}
-
-
-/**
- * Returns to the user the value of the right "incremental" encoder.
- *
- * @return The value of "tics" of the right encoder since it was initialized
- */
-long int R_encoder()
-{
- wait(0.0001);
-
- return total_R;
-}
-
-/**
- * Returns to the user the value of the right "incremental" encoder.
- *
- * @return The value of "tics" of the right encoder since it was initialized
- */
-long int L_encoder()
-{
- wait(0.0001);
-
- return total_L;
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// BATTERY ///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -425,15 +301,14 @@
* Reads adc of the battery.
*
* @param addr - Address to read
- * @return The voltage of the batery
+ * @return The voltage of the baterry
*/
long int read16_mcp3424(char addr)
{
char data[4];
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.read(addr,data,3);
- i2c1.stop();
-
+ mutex_i2c1.unlock();
return(((data[0]&127)*256)+data[1]);
}
@@ -464,9 +339,9 @@
char data[1];
data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.write(addr,data,1);
- i2c1.stop();
+ mutex_i2c1.unlock();
}
@@ -491,179 +366,6 @@
}
///////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////// RF COMUNICATION ///////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-
-/**
- * Initializes the NRF24 module for comunication.
- *
- * Note: if the module is broken or badly connected this init will cause the code to stop,
- * if all these messages don't appear thats the case
- */
-void config_init_nrf()
-{
- my_nrf24l01p.powerUp(); // powers module
- my_nrf24l01p.setRfFrequency (2400); // channel 0 (2400-0 ... 2516-116)
- my_nrf24l01p.setTransferSize(10); // number of bytes to be transfer
- my_nrf24l01p.setCrcWidth(8);
- my_nrf24l01p.enableAutoAcknowledge(NRF24L01P_PIPE_P0); // pipe where data transfer occurs (0..6)
- pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() );
- pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
- pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
- pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
- pc.printf( "nRF24L01+ CrC Width : %d CrC\r\n", my_nrf24l01p.getCrcWidth() );
- pc.printf( "nRF24L01+ TransferSize : %d Paket Size\r\n", my_nrf24l01p.getTransferSize () );
- my_nrf24l01p.setReceiveMode();
- my_nrf24l01p.enable();
- pc.printf( "Setup complete, Starting While loop\r\n");
-}
-
-
-/**
- * Receives a number from the Arduino.
- *
- * @return The value send by the arduino
- */
-double receiveValue(void)
-{
- char temp[4];
- double Val;
- bool ok=0;
- my_nrf24l01p.setTransferSize(4);
- my_nrf24l01p.setReceiveMode();
- my_nrf24l01p.enable();
- do {
- if(my_nrf24l01p.readable(NRF24L01P_PIPE_P0)) {
- ok = my_nrf24l01p.read( NRF24L01P_PIPE_P0,temp, 4);
- }
- } while(ok==0);
-
- //transformation of temp to convert to original value
- if(temp[0]==0) // if first elemente is 0 then its negative
- Val = double(-(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255));
- else // else its positive
- Val = double(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255);
-
- return Val;
-}
-
-
-/**
- * Sends a number to the Arduino
- *
- * @param Value - number to be sent to the Arduino
- */
-void sendValue(long int Value)
-{
- bool ok=0; // comunication sucess, o if failed 1 if sucessfull
- // double math=Value/65025; // temporary variable save results
- int zero=1; // 1 byte, ( - ) if 0 ( + ) if 1
- int one=0; // 2 byte (0..255), multiplied by 1
- int two=0; // 3 byte (0..255), multiplied by 255
- int three=0; // 4 byte (0..255), multiplied by 255*255
-
-//transformation of Value to send correctly through pipe
- if (Value<0) {
- zero=0;
- Value=abs(Value);
- }
- // Value=abs(Value);
-
- double math=Value/65025; // temporary variable save results
-
- if(math<1) {
- math=Value/255;
- if(math<1) {
- two=0;
- one=Value;
- } else {
- two=(int)math;
- one=Value % 255;
- }
- } else {
- three=(int)math;
- math=Value/255;
- if(math<1) {
- two=0;
- one=Value;
- } else {
- two=(int)math;
- one=Value % 255;
- }
- }
- char temp[4]= {(int)zero,(int)one,(int)two,(int)three};
-
- // Apagar depois de testar mais vezes
- // pc.printf("1 inidice...%i...\r", temp[0]);
- // pc.printf("2 inidice...%i...\r", temp[1]);
- // pc.printf("3 inidice...%i...\r", temp[2]);
- // pc.printf("4 inidice...%i...\r", temp[3]);
-
- my_nrf24l01p.setTransferSize(4);
- my_nrf24l01p.setTransmitMode();
- my_nrf24l01p.enable();
- do {
- ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,temp, 4);
- if(ok==1)
- pc.printf("Done.....%i...\r", Value);
- else {
- pc.printf("Failed\r");
- wait(1);
- }
- } while(ok==0);
-}
-
-/**
- * Sends matrix to arduino.
- *
- * @param matrix - Matrix of numbers to send [0..255]
- * @param row - Number of rows
- * @param column - Number of columns
- */
-void sendMatrix(int (*matrix)[18],int row , int column)
-{
- short ok=0;
- short int i =0;
- short int j=0;
- short int byte=0;
- int members=column*row;
- char message[32]= {0};
- pc.printf("J ...%d... \r",members);
-
- my_nrf24l01p.setTransferSize(32);
- my_nrf24l01p.setTransmitMode();
- my_nrf24l01p.enable();
-
- do {
- int* point = matrix[j];
-
- do {
- message[byte]= point[i];
- if (byte==31 || (i+1)*(j+1)==members) {
-
- do {
- ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,message, 32);
- if(ok==0)
- wait(1);
-
- } while(ok==0);
-
- byte=-1;
- }
-
- byte++;
- i++;
-
- } while(i<column);
-
- i=0;
- j++;
- } while(j<row);
-
-}
-
-///////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// Sonar ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
// Commands of operation with ultrasonic module
@@ -688,57 +390,9 @@
{
char data[1];
data[0]= 0x0C;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data,1);
- i2c1.stop();
- i2c1.start();
- i2c1.write(0x30,data,1);
- i2c1.stop();
-}
-
-
-void disable_dc_dc_boost()
-{
- char data[1];
- data[0]= 0x0B;
- wait_ms(1);
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.write(0x30,data,1);
- i2c1.stop();
-}
-
-
-void start_read_left_sensor()
-{
- char data[1];
- data[0]= 0x0A;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data,1);
- i2c1.stop();
-}
-
-
-void start_read_front_sensor()
-{
- char data[1];
- data[0]= 0x09;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data,1);
- i2c1.stop();
-}
-
-
-void start_read_right_sensor()
-{
- char data[1];
- data[0]= 0x08;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data,1);
- i2c1.stop();
+ mutex_i2c1.unlock();
}
@@ -746,49 +400,30 @@
{
char data[1];
data[0]= 0x07;
- wait_ms(1);
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.write(0x30,data,1);
- i2c1.stop();
+ mutex_i2c1.unlock();
}
-void measure_always_off()
-{
- char data[1];
- data[0]= 0x06;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data,1);
- i2c1.stop();
-}
-
/**
* Returns left sensor value
*/
static unsigned int get_distance_left_sensor()
{
-
+
static char data_r[3];
static unsigned int aux;
- flag=1;
-
+
data_r[0]= 0x05;
- wait_ms(1);
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
- i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+ i2c1.read(0x31,data_r,2);
+ mutex_i2c1.unlock();
aux=(data_r[0]*256)+data_r[1];
- flag=0;
+
return aux;
- // sensor_left=aux;
- // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
-
}
@@ -800,22 +435,17 @@
static char data_r[3];
static unsigned int aux;
- flag=1;
+
data_r[0]= 0x04;
- wait_ms(1);
- i2c1.start();
+
+ mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
- i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+ i2c1.read(0x31,data_r,2);
+ mutex_i2c1.unlock();
aux=(data_r[0]*256)+data_r[1];
- flag=0;
+
return aux;
- // sensor_front=aux;
- // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
}
@@ -828,69 +458,359 @@
static char data_r[3];
static unsigned int aux;
- flag=1;
data_r[0]= 0x03;
- wait_ms(1);
- i2c1.start();
+
+ mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+ mutex_i2c1.unlock();
aux=(data_r[0]*256)+data_r[1];
- flag=0;
+
return aux;
- // sensor_right=aux;
- // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
}
-void get_status_always_measure()
-{
+///////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////// RF COMMUNICATION ////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
- static char data_r[3];
- static unsigned int aux;
+/**
+ * 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];
+
- data_r[0]= 0x02;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
- i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+/**
+ * Wireless control of motors using nrf24l01 module and FRDM-KL25Z board(robot is working as receiver).
+ */
+void robot_RC()
+{
+ if (radio.available() ) {
- aux=data_r[0];
- pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
-
+ 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);
+ }
+ }
}
-void get_status_dcdc_converter()
-{
+
+/**
+ * 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
- static char data_r[3];
- static unsigned int aux;
+ 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
- data_r[0]= 0x01;
- wait_ms(1);
- i2c1.start();
- i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
- i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+
+// 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
- aux=data_r[0];
- pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
+ 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();
+ }
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -958,70 +878,42 @@
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(void)
-{
+void initRobot(int channel)
+{
+ pc.printf("Battery level: \n\r");
+ init_nRF(channel);
init_robot_pins();
enable_dc_dc_boost();
- init_Infrared();
- initEncoders();
- config_init_nrf();
+ wait_ms(100); //wait for read wait(>=150ms);
enable_dc_dc_boost();
+ measure_always_on();
wait_ms(100); //wait for read wait(>=150ms);
- measure_always_on();
+
+ 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));
-}
-
-
-////////////////////////////////////////////////////
-
-/**
- * Updates the position and orientation of the robot based on the data from the encoders
- *
- * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
- * and the distance between them is 7.4
- */
-void Odometria()
-{
- long int ticks1d=R_encoder();
- long int ticks1e=L_encoder();
-
- 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);
-
}
\ No newline at end of file