Fabio Faria / ISR_Mini-explorer_tmp2

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

Dependencies:   RF24

Dependents:   ISR_Mini-explorer_Rangefinder

Fork of ISR_Mini-explorer by ISR UC

Radio.cpp@6:86bccb8afac3, 2018-05-14 (annotated)

Committer:
fabiofaria
Date:
Mon May 14 14:53:10 2018 +0000
Revision:
6:86bccb8afac3
Parent:
4:f6fddeae358e
Child:
7:95bee84b6910
read_rangefinder added.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabiofaria 4:f6fddeae358e 1 #include "Radio.h"
fabiofaria 4:f6fddeae358e 2 #include "mbed.h"
fabiofaria 4:f6fddeae358e 3 #include "Rangefinder.h"
fabiofaria 4:f6fddeae358e 4
fabiofaria 4:f6fddeae358e 5 RF24 radio(PTD2, PTD3, PTD1, PTC12 ,PTC13);
fabiofaria 4:f6fddeae358e 6 const char max_len = 30;
fabiofaria 4:f6fddeae358e 7
fabiofaria 4:f6fddeae358e 8 MBED_PACKED(struct) BasicFrame { // MBED_PACKED is required to avoid unpredictable data sizes due to Padding.
fabiofaria 4:f6fddeae358e 9 char start_of_frame_1;
fabiofaria 4:f6fddeae358e 10 char start_of_frame_2;
fabiofaria 4:f6fddeae358e 11 char identifier;
fabiofaria 4:f6fddeae358e 12 } BasicFrame1;
fabiofaria 4:f6fddeae358e 13
fabiofaria 4:f6fddeae358e 14 MBED_PACKED(struct) MatrixFrame { // MBED_PACKED is required to avoid unpredictable data sizes due to Padding.
fabiofaria 4:f6fddeae358e 15 char start_of_frame_1;
fabiofaria 4:f6fddeae358e 16 char start_of_frame_2;
fabiofaria 4:f6fddeae358e 17 char identifier;
fabiofaria 4:f6fddeae358e 18 char dim1;
fabiofaria 4:f6fddeae358e 19 char dim2;
fabiofaria 4:f6fddeae358e 20 } MatrixFrame1;
fabiofaria 4:f6fddeae358e 21
fabiofaria 4:f6fddeae358e 22 /**
fabiofaria 4:f6fddeae358e 23 * Initializes nrf24l01 module, return value 1 if module is working correcty and 0 if it's not.
fabiofaria 4:f6fddeae358e 24 *
fabiofaria 4:f6fddeae358e 25 ** @param channel - Which RF channel to comunicate on, 0-125
fabiofaria 4:f6fddeae358e 26 *
fabiofaria 4:f6fddeae358e 27 * \warning Channel on Robot and Board has to be the same.
fabiofaria 4:f6fddeae358e 28 */
fabiofaria 4:f6fddeae358e 29 bool init_nRF(int channel)
fabiofaria 4:f6fddeae358e 30 {
fabiofaria 4:f6fddeae358e 31 bool result;
fabiofaria 4:f6fddeae358e 32
fabiofaria 4:f6fddeae358e 33 result = radio.begin();
fabiofaria 4:f6fddeae358e 34 //pc.printf( "Initialation nrf24l01=%d\r\n", result ); // 1-working,0-not working
fabiofaria 4:f6fddeae358e 35 radio.setDataRate(RF24_250KBPS);
fabiofaria 4:f6fddeae358e 36 radio.setCRCLength(RF24_CRC_16);
fabiofaria 4:f6fddeae358e 37 radio.enableDynamicPayloads();
fabiofaria 4:f6fddeae358e 38 radio.setChannel(channel);
fabiofaria 4:f6fddeae358e 39 radio.setAutoAck(true);
fabiofaria 4:f6fddeae358e 40
fabiofaria 4:f6fddeae358e 41 radio.openWritingPipe(0x314e6f6465);
fabiofaria 4:f6fddeae358e 42 radio.openReadingPipe(1,0x324e6f6465);
fabiofaria 4:f6fddeae358e 43
fabiofaria 4:f6fddeae358e 44 radio.startListening();
fabiofaria 4:f6fddeae358e 45
fabiofaria 4:f6fddeae358e 46 return result;
fabiofaria 4:f6fddeae358e 47 }
fabiofaria 4:f6fddeae358e 48
fabiofaria 4:f6fddeae358e 49 void radio_send_string(char *str)
fabiofaria 4:f6fddeae358e 50 {
fabiofaria 4:f6fddeae358e 51 char *ptr;
fabiofaria 4:f6fddeae358e 52
fabiofaria 4:f6fddeae358e 53 ptr = str;
fabiofaria 4:f6fddeae358e 54
fabiofaria 4:f6fddeae358e 55 while(strlen(ptr) > max_len) {
fabiofaria 4:f6fddeae358e 56 radio.stopListening();
fabiofaria 4:f6fddeae358e 57 radio.write(ptr, max_len);
fabiofaria 4:f6fddeae358e 58 radio.startListening();
fabiofaria 4:f6fddeae358e 59 ptr += max_len;
fabiofaria 4:f6fddeae358e 60 }
fabiofaria 4:f6fddeae358e 61
fabiofaria 4:f6fddeae358e 62 radio.stopListening();
fabiofaria 4:f6fddeae358e 63 radio.write(ptr, strlen(ptr));
fabiofaria 4:f6fddeae358e 64 radio.startListening();
fabiofaria 4:f6fddeae358e 65 }
fabiofaria 4:f6fddeae358e 66
fabiofaria 4:f6fddeae358e 67 void send_float_matrix(float *matrix, unsigned num_lines, unsigned num_columns)
fabiofaria 4:f6fddeae358e 68 {
fabiofaria 4:f6fddeae358e 69 int num_bytes_to_send;
fabiofaria 4:f6fddeae358e 70 uint8_t *ptr;
fabiofaria 4:f6fddeae358e 71
fabiofaria 4:f6fddeae358e 72 // Send frame structure.
fabiofaria 4:f6fddeae358e 73 MatrixFrame1.start_of_frame_1 = 0x06;
fabiofaria 4:f6fddeae358e 74 MatrixFrame1.start_of_frame_2 = 0x85;
fabiofaria 4:f6fddeae358e 75 MatrixFrame1.identifier = 'F';
fabiofaria 4:f6fddeae358e 76 MatrixFrame1.dim1 = num_lines;
fabiofaria 4:f6fddeae358e 77 MatrixFrame1.dim2 = num_columns;
fabiofaria 4:f6fddeae358e 78
fabiofaria 4:f6fddeae358e 79 radio.stopListening();
fabiofaria 4:f6fddeae358e 80 radio.write(&MatrixFrame1, sizeof(MatrixFrame1));
fabiofaria 4:f6fddeae358e 81
fabiofaria 4:f6fddeae358e 82 // Send matrix.
fabiofaria 4:f6fddeae358e 83 ptr = (uint8_t*)matrix;
fabiofaria 4:f6fddeae358e 84 num_bytes_to_send = num_lines * num_columns * sizeof(float);
fabiofaria 4:f6fddeae358e 85
fabiofaria 4:f6fddeae358e 86 while(num_bytes_to_send > max_len) {
fabiofaria 4:f6fddeae358e 87 radio.write(ptr, max_len); // It would be helpful a blocking function. write() should block, but it appears been modified.
fabiofaria 4:f6fddeae358e 88 ptr += max_len;
fabiofaria 4:f6fddeae358e 89 num_bytes_to_send -= max_len;
fabiofaria 4:f6fddeae358e 90 }
fabiofaria 4:f6fddeae358e 91
fabiofaria 4:f6fddeae358e 92 radio.write(ptr, num_bytes_to_send);
fabiofaria 4:f6fddeae358e 93 radio.startListening();
fabiofaria 4:f6fddeae358e 94 }
fabiofaria 4:f6fddeae358e 95
fabiofaria 4:f6fddeae358e 96 void send_rangefinder()
fabiofaria 4:f6fddeae358e 97 {
fabiofaria 4:f6fddeae358e 98 BasicFrame1.start_of_frame_1 = 0x06;
fabiofaria 4:f6fddeae358e 99 BasicFrame1.start_of_frame_2 = 0x85;
fabiofaria 4:f6fddeae358e 100 BasicFrame1.identifier = 'R';
fabiofaria 4:f6fddeae358e 101
fabiofaria 4:f6fddeae358e 102 radio.stopListening();
fabiofaria 4:f6fddeae358e 103 radio.write(&BasicFrame1, sizeof(BasicFrame1));
fabiofaria 4:f6fddeae358e 104 radio.write(rgfMeasurements, sizeof(rgfMeasurements));
fabiofaria 4:f6fddeae358e 105 radio.startListening();
fabiofaria 4:f6fddeae358e 106 }
fabiofaria 4:f6fddeae358e 107
fabiofaria 4:f6fddeae358e 108 void test_send_matrix()
fabiofaria 4:f6fddeae358e 109 {
fabiofaria 4:f6fddeae358e 110 const int lines_num = 15, col_num = 15;
fabiofaria 4:f6fddeae358e 111 float testmatrix[lines_num][col_num];
fabiofaria 4:f6fddeae358e 112 int i, j;
fabiofaria 4:f6fddeae358e 113 float previous = 0.1;
fabiofaria 4:f6fddeae358e 114
fabiofaria 4:f6fddeae358e 115 for(i=0; i<lines_num; i++) {
fabiofaria 4:f6fddeae358e 116 for(j=0; j<col_num; j++) {
fabiofaria 4:f6fddeae358e 117 testmatrix[i][j] = previous;
fabiofaria 4:f6fddeae358e 118 previous += 0.1;
fabiofaria 4:f6fddeae358e 119 }
fabiofaria 4:f6fddeae358e 120 }
fabiofaria 4:f6fddeae358e 121
fabiofaria 4:f6fddeae358e 122 send_float_matrix(&testmatrix[0][0], lines_num, col_num);
fabiofaria 6:86bccb8afac3 123 }
fabiofaria 6:86bccb8afac3 124
fabiofaria 6:86bccb8afac3 125 void read_rangefinder(char* buf, uint8_t array_size)
fabiofaria 6:86bccb8afac3 126 {
fabiofaria 6:86bccb8afac3 127 uint8_t payload_len;
fabiofaria 6:86bccb8afac3 128 int available;
fabiofaria 6:86bccb8afac3 129 char* buf2;
fabiofaria 6:86bccb8afac3 130
fabiofaria 6:86bccb8afac3 131 buf2 = buf;
fabiofaria 6:86bccb8afac3 132 available = array_size;
fabiofaria 6:86bccb8afac3 133
fabiofaria 6:86bccb8afac3 134 while (radio.available()) {
fabiofaria 6:86bccb8afac3 135 // Fetch the payload.
fabiofaria 6:86bccb8afac3 136 payload_len = radio.getDynamicPayloadSize();
fabiofaria 6:86bccb8afac3 137
fabiofaria 6:86bccb8afac3 138 // If a corrupt dynamic payload is received, flush it.
fabiofaria 6:86bccb8afac3 139 if (!payload_len)
fabiofaria 6:86bccb8afac3 140 continue;
fabiofaria 6:86bccb8afac3 141
fabiofaria 6:86bccb8afac3 142 if (available >= payload_len) {
fabiofaria 6:86bccb8afac3 143 radio.read(buf2, payload_len);
fabiofaria 6:86bccb8afac3 144 buf2 += payload_len;
fabiofaria 6:86bccb8afac3 145 available -= payload_len;
fabiofaria 6:86bccb8afac3 146 } else {
fabiofaria 6:86bccb8afac3 147 break;
fabiofaria 6:86bccb8afac3 148 }
fabiofaria 6:86bccb8afac3 149 }
fabiofaria 4:f6fddeae358e 150 }