Revision 4:560d24c0e5f8, committed 2018-04-19

Comitter:

fabiofaria

Date:

Thu Apr 19 15:35:29 2018 +0000

Parent:

3:215c9544480d

Commit message:

Initial commit.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Radio.cpp	Thu Apr 19 15:35:29 2018 +0000
@@ -0,0 +1,123 @@
+#include "Radio.h"
+#include "mbed.h"
+#include "Rangefinder.h"
+
+RF24 radio(PTD2, PTD3, PTD1,  PTC12 ,PTC13);
+const char max_len = 30;
+
+MBED_PACKED(struct) BasicFrame { // MBED_PACKED is required to avoid unpredictable data sizes due to Padding.
+    char start_of_frame_1;
+    char start_of_frame_2;
+    char identifier;
+} BasicFrame1;
+
+MBED_PACKED(struct) MatrixFrame { // MBED_PACKED is required to avoid unpredictable data sizes due to Padding.
+    char start_of_frame_1;
+    char start_of_frame_2;
+    char identifier;
+    char dim1;
+    char dim2;
+} MatrixFrame1;
+
+/**
+ * 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.
+ */
+bool init_nRF(int channel)
+{
+    bool result;
+
+    result = radio.begin();
+    //pc.printf( "Initialation nrf24l01=%d\r\n",  result ); // 1-working,0-not working
+    radio.setDataRate(RF24_250KBPS);
+    radio.setCRCLength(RF24_CRC_16);
+    radio.enableDynamicPayloads();
+    radio.setChannel(channel);
+    radio.setAutoAck(true);
+
+    radio.openWritingPipe(0x314e6f6465);
+    radio.openReadingPipe(1,0x324e6f6465);
+
+    radio.startListening();
+
+    return result;
+}
+
+void radio_send_string(char *str)
+{
+    char *ptr;
+
+    ptr = str;
+
+    while(strlen(ptr) > max_len) {
+        radio.stopListening();
+        radio.write(ptr, max_len);
+        radio.startListening();
+        ptr += max_len;
+    }
+
+    radio.stopListening();
+    radio.write(ptr, strlen(ptr));
+    radio.startListening();
+}
+
+void send_float_matrix(float *matrix, unsigned num_lines, unsigned num_columns)
+{
+    int num_bytes_to_send;
+    uint8_t *ptr;
+
+    // Send frame structure.
+    MatrixFrame1.start_of_frame_1 = 0x06;
+    MatrixFrame1.start_of_frame_2 = 0x85;
+    MatrixFrame1.identifier = 'F';
+    MatrixFrame1.dim1 = num_lines;
+    MatrixFrame1.dim2 = num_columns;
+
+    radio.stopListening();
+    radio.write(&MatrixFrame1, sizeof(MatrixFrame1));
+
+    // Send matrix.
+    ptr = (uint8_t*)matrix;
+    num_bytes_to_send = num_lines * num_columns * sizeof(float);
+
+    while(num_bytes_to_send > max_len) {
+        radio.write(ptr, max_len); // It would be helpful a blocking function. write() should block, but it appears been modified.
+        ptr += max_len;
+        num_bytes_to_send -= max_len;
+    }
+
+    radio.write(ptr, num_bytes_to_send);
+    radio.startListening();
+}
+
+void send_rangefinder()
+{
+    BasicFrame1.start_of_frame_1 = 0x06;
+    BasicFrame1.start_of_frame_2 = 0x85;
+    BasicFrame1.identifier = 'R';
+
+    radio.stopListening();
+    radio.write(&BasicFrame1, sizeof(BasicFrame1));
+    radio.write(rgfMeasurements, sizeof(rgfMeasurements));
+    radio.startListening();
+}
+
+void test_send_matrix()
+{
+    const int lines_num = 15, col_num = 15;
+    float testmatrix[lines_num][col_num];
+    int i, j;
+    float previous = 0.1;
+
+    for(i=0; i<lines_num; i++) {
+        for(j=0; j<col_num; j++) {
+            testmatrix[i][j] = previous;
+            previous += 0.1;
+        }
+    }
+
+    send_float_matrix(&testmatrix[0][0], lines_num, col_num);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Radio.h	Thu Apr 19 15:35:29 2018 +0000
@@ -0,0 +1,15 @@
+#ifndef RADIO_H
+#define RADIO_H
+
+#include "mbed.h"
+#include "RF24.h"
+
+extern RF24 radio;
+
+bool init_nRF(int channel);
+void radio_send_string(char *str);
+void send_float_matrix(float *matrix, unsigned num_lines, unsigned num_columns);
+void send_rangefinder();
+void test_send_matrix();
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rangefinder.cpp	Thu Apr 19 15:35:29 2018 +0000
@@ -0,0 +1,33 @@
+#include "Rangefinder.h"
+#include "mbed.h"
+
+const int rgfAddr = 0x14;
+uint16_t rgfMeasurements[16];
+//char sensors_selection[] = {1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0}; // Only allows a maximum of 11 sensors simultaneously.
+//char sensors_selection[] = {1,0,0,0,1,0,1,0,1,0,1,0,1,0,1,0}; // Only allows a maximum of 11 sensors simultaneously.
+char sensors_selection[] = {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; // Only allows a maximum of 11 sensors simultaneously.
+
+I2C* _i2c;
+Mutex* _mutex;
+
+void init_rangefinder(I2C* i2c_in, Mutex* mutex_in)
+{
+    _i2c = i2c_in;
+    _mutex = mutex_in;
+    wait(1);
+    _mutex->lock();
+    _i2c->write(rgfAddr, sensors_selection, 16);
+    _mutex->unlock();
+    wait(1);
+}
+
+int read_rangefinder()
+{
+    int i2c_result;
+
+    _mutex->lock();
+    i2c_result = _i2c->read(rgfAddr, (char*)rgfMeasurements, sizeof(rgfMeasurements));
+    _mutex->unlock();
+
+    return i2c_result;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rangefinder.h	Thu Apr 19 15:35:29 2018 +0000
@@ -0,0 +1,12 @@
+#ifndef RANGEFINDER_H
+#define RANGEFINDER_H
+
+#include "mbed.h"
+
+extern uint16_t rgfMeasurements[16];
+extern char sensors_selection[16];
+
+void init_rangefinder(I2C* i2c_in, Mutex* mutex_in);
+int read_rangefinder();
+
+#endif
\ No newline at end of file

--- a/robot.h	Fri Mar 16 16:49:02 2018 +0000
+++ b/robot.h	Thu Apr 19 15:35:29 2018 +0000
@@ -2,26 +2,22 @@
 
 #include "mbed.h"
 #include <math.h>
-#include <string.h>
 #include "VCNL40x0.h"
-#include <RF24.h>
+#include "Radio.h"
 #include "Encoder.h"
+#include "Rangefinder.h"
 
 I2C i2c(PTC9,PTC8);
 I2C i2c1(PTC11,PTC10);
-
+Mutex mutex_i2c0;
+Mutex mutex_i2c1;
 
-    float X=20;
-    float Y=20;
-    float THETA;
-    
-    Mutex mutex_i2c0, mutex_i2c1;
-    Encoder esquerdo(&i2c, &mutex_i2c0, 0);
-    Encoder direito(&i2c1, &mutex_i2c1, 1);
-    
-    Thread thread;
-    
-
+float X=20;
+float Y=20;
+float THETA;
+Encoder esquerdo(&i2c, &mutex_i2c0, 0);
+Encoder direito(&i2c1, &mutex_i2c1, 1);
+Thread thread;
 
 void odometry_thread()
 {
@@ -64,14 +60,8 @@
     }
 }
 
-
 // classes adicionais
-RF24 radio(PTD2, PTD3, PTD1,  PTC12 ,PTC13);
 VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
-
-
-Timeout timeout;
-
 Serial pc(PTE0,PTE1);
 
 
@@ -137,7 +127,6 @@
 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
@@ -476,54 +465,6 @@
 }
 
 
-///////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////     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_250KBPS);
-      radio.setCRCLength(RF24_CRC_16);
-      radio.enableDynamicPayloads();
-      radio.setChannel(channel);
-      radio.setAutoAck(true);
-    
-      radio.openWritingPipe(0x314e6f6465);
-      radio.openReadingPipe(1,0x324e6f6465);
-    
-      radio.startListening();
-}
-
-char q[1];
-
-void radio_send_string(char *str)
-{
-    const int max_len = 30;
-    char *ptr;
-
-    ptr = str;
-
-    while(strlen(ptr) > max_len) {
-        radio.stopListening();
-        radio.write(ptr, max_len);
-        radio.startListening();
-        ptr += max_len;
-    }
-
-    radio.stopListening();
-    radio.write(ptr, strlen(ptr));
-    radio.startListening();
-}
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////      MISC.      ////////////////////////////////////////////
@@ -618,7 +559,6 @@
    
     init_Infrared();
     
-    
     thread.start(odometry_thread);
     
     float value = value_of_Batery();