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:
- 3:215c9544480d
- Parent:
- 2:0435d1171673
- Child:
- 4:560d24c0e5f8
diff -r 0435d1171673 -r 215c9544480d robot.h
--- a/robot.h Tue Feb 20 17:17:41 2018 +0000
+++ b/robot.h Fri Mar 16 16:49:02 2018 +0000
@@ -1,5 +1,6 @@
/** @file */
+#include "mbed.h"
#include <math.h>
#include <string.h>
#include "VCNL40x0.h"
@@ -491,9 +492,9 @@
result = radio.begin();
pc.printf( "Initialation nrf24l01=%d\r\n", result ); // 1-working,0-not working
- radio.setDataRate(RF24_1MBPS);
+ radio.setDataRate(RF24_250KBPS);
radio.setCRCLength(RF24_CRC_16);
- radio.setPayloadSize(32);
+ radio.enableDynamicPayloads();
radio.setChannel(channel);
radio.setAutoAck(true);
@@ -503,317 +504,26 @@
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()
+void radio_send_string(char *str)
{
- 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
+ const int max_len = 30;
+ char *ptr;
- 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
+ ptr = str;
- 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//////
+ while(strlen(ptr) > max_len) {
radio.stopListening();
- radio.write( &send_Z, sizeof(send_Z));
- radio.write( &send_Q, sizeof(send_Q) );
+ radio.write(ptr, max_len);
radio.startListening();
+ ptr += max_len;
}
+ radio.stopListening();
+ radio.write(ptr, strlen(ptr));
+ radio.startListening();
+}
///////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// MISC. ////////////////////////////////////////////
@@ -899,6 +609,7 @@
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();
@@ -906,10 +617,12 @@
wait_ms(100); //wait for read wait(>=150ms);
init_Infrared();
- float value = value_of_Batery();
+
thread.start(odometry_thread);
+ float value = value_of_Batery();
+
pc.printf("Initialization Successful \n\r");
pc.printf("Battery level: %f \n\r",value);
if(value < 3.0) {