현성 김 / Mbed 2 deprecated 181202_Castone_design_master

Dependencies:   mbed nRF24L01P

Branch:
RF24_library_test_tx
Revision:
19:6f01eb6f4163
Child:
20:e0302dc43412
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/master.cpp	Sat Dec 01 19:32:07 2018 +0000
@@ -0,0 +1,253 @@
+#include "mbed.h"
+#include "nRF24L01P.h"
+#include "beep.h"
+
+#define nrf_CE      D2
+#define nrf_CSN     A3
+#define spi_SCK     D13
+#define spi_MOSI    D11
+#define spi_MISO    D12
+#define spi_IRQ     D4
+
+#define TRANSFER_SIZE 15
+#define MAX_BUFFER_SIZE 255
+#define BUFFER_SIZE MAX_BUFFER_SIZE
+
+#define DATA_PROTOCOL_BEGIN '$'
+#define DATA_PROTOCOL_TOKEN ','
+#define DATA_PROTOCOL_TOKEN_ ","
+#define DATA_PROTOCOL_END   '*'
+#define DATA_PROTOCOL_END_  "*"
+
+#define NRF_TRANSMIT    1
+#define NRF_RECEIVE     2
+
+#define NRF_RETRIAL_MAX 5
+
+nRF24L01P nrf(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE, spi_IRQ);    // mosi, miso, sck, csn, ce, irq
+Serial pc(USBTX, USBRX);
+PwmOut buzzer(D5);
+
+uint64_t txAddr = 0xDEADBEEF0F;
+uint64_t rxAddr = 0xDEADBEEF00;
+
+typedef struct {
+    int bot_id;
+    int cmd_id;
+    int lspeed;
+    int rspeed;
+} payload_t;
+
+void beepStart();
+void endBeep();
+void initNRF(int mode, int msg_len);
+void dumpRFInfo();
+int readSerialUntil(char *buffer, unsigned int buf_len, char terminator);
+char* inspectData(char *buffer, int buf_len);
+payload_t* getPayload(char* raw_data);
+int transmitCmd(payload_t *p);
+
+int main()
+{
+    char buffer[BUFFER_SIZE];
+    beepStart();
+    pc.baud(115200);
+    printf("I'm Master\r\n");
+    initNRF(NRF_TRANSMIT, TRANSFER_SIZE);
+//    dumpRFInfo();
+
+    memset(buffer, NULL, BUFFER_SIZE);
+
+    while(1) {
+        int result = readSerialUntil(buffer, BUFFER_SIZE, DATA_PROTOCOL_END);
+        if(result>0) {
+            char *data = inspectData(buffer, BUFFER_SIZE);
+            if(*data == DATA_PROTOCOL_BEGIN) {
+                payload_t *payload = getPayload(data);
+                printf("Sent - BotID:%d, CmdID:%d, LSpeed:%d, RSpeed:%d\r\n", payload->bot_id, payload->cmd_id, payload->lspeed, payload->rspeed);
+                int n = transmitCmd(payload);
+                free(data);
+                free(payload);
+            }
+        }
+    }
+}
+
+void constructString(char *txData, payload_t *p)
+{
+    *(txData+0) = '$';
+    *(txData+1) = p->bot_id/10+'0';
+    *(txData+2) = p->bot_id%10+'0';
+    *(txData+3) = p->cmd_id/10000+'0';
+    *(txData+4) = p->cmd_id/1000%10+'0';
+    *(txData+5) = p->cmd_id/100%10+'0';
+    *(txData+6) = p->cmd_id/10%10+'0';
+    *(txData+7) = p->cmd_id%10+'0';
+    *(txData+8) = p->lspeed>0?'+':'-';
+    *(txData+9) = abs(p->lspeed)/10+'0';
+    *(txData+10) = abs(p->lspeed)%10+'0';
+    *(txData+11) = p->rspeed>0?'+':'-';
+    *(txData+12) = abs(p->rspeed)/10+'0';
+    *(txData+13) = abs(p->rspeed)%10+'0';
+    *(txData+14) = '\0';
+}
+
+int transmitCmd(payload_t *p)
+{
+    int trial = 0;
+    int rxDataCnt=0;
+    char rxData[MAX_BUFFER_SIZE];
+    char buf[TRANSFER_SIZE];
+    
+    while(trial<NRF_RETRIAL_MAX) {
+        initNRF(NRF_TRANSMIT, TRANSFER_SIZE);
+        constructString(buf, p);
+
+        nrf.write(NRF24L01P_PIPE_P0, buf, TRANSFER_SIZE);
+        initNRF(NRF_RECEIVE, TRANSFER_SIZE);
+        wait(0.01);
+        rxDataCnt = nrf.read( NRF24L01P_PIPE_P0, rxData, TRANSFER_SIZE);
+        if(rxDataCnt>0) {
+            return 1;
+        }
+        trial++;
+    }
+    printf("No Reply from Bot..\r\n");
+    return 0;
+}
+
+payload_t* getPayload(char *data)
+{
+    int n = strlen(data);
+    char _data[n+1];
+    strcpy(_data, data);
+    payload_t *p = (payload_t*)malloc(sizeof(payload_t));
+    p->bot_id = atoi(strtok(data+1, DATA_PROTOCOL_TOKEN_));
+    p->cmd_id = atoi(strtok(NULL, DATA_PROTOCOL_TOKEN_));
+    p->lspeed = atoi(strtok(NULL, DATA_PROTOCOL_TOKEN_));
+    p->rspeed = atoi(strtok(NULL, DATA_PROTOCOL_END_));
+    return p;
+}
+
+char* inspectData(char *buffer, int buf_len)
+{
+    int start, end;
+    for(int i=0; buffer[i-1]!=DATA_PROTOCOL_BEGIN; i++) {
+        start = i;
+        if(i==buf_len) return 0;
+    }
+    for(int i=start+1; buffer[i-1]!=DATA_PROTOCOL_END; i++) {
+        end = i;
+        if(i==buf_len) return 0;
+    }
+    int data_len = end-start+1;
+    char *str = (char*)malloc(sizeof(char)*(data_len+1));
+    memset(str, NULL, data_len+1);
+    for(int i=0; i<data_len; i++) {
+        *(str+i) = buffer[start+i];
+
+        if(buffer[start+i]==DATA_PROTOCOL_BEGIN || buffer[start+i]==DATA_PROTOCOL_TOKEN || buffer[start+i]==DATA_PROTOCOL_END
+                || (buffer[start+i]>='0'&&buffer[start+i]<='9') || buffer[start+i]=='+' || buffer[start+i]=='-') {
+            continue;
+        } else {
+            //pc.putc(buffer[start+i]);
+            free(str);
+            return 0;
+        }
+    }
+    return str;
+}
+
+int readSerialUntil(char *buffer, unsigned int buf_len, char term)
+{
+    int n=0;
+    char *str = (char*)malloc(sizeof(char)*buf_len);
+    memset(str, NULL, buf_len);
+    Timer t;
+    t.start();
+    int begin = t.read_ms();
+    while(t.read_ms()-begin < 100) {
+        if(pc.readable()) {
+            char inByte=pc.getc();
+            *(str+n)=inByte;
+            n++;
+            if(inByte == term) {
+                strcpy(buffer, str);
+                free(str);
+                return n;
+            }
+            if(n>buf_len) {
+                free(str);
+                return -1;
+            }
+        }
+    }
+    free(str);
+    return 0;
+}
+
+void dumpRFInfo()
+{
+    printf( "nRF24L01+ Frequency    : %d MHz\r\n",  nrf.getRfFrequency() );
+    printf( "nRF24L01+ Output power : %d dBm\r\n",  nrf.getRfOutputPower() );
+    printf( "nRF24L01+ Data Rate    : %d kbps\r\n", nrf.getAirDataRate() );
+    printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", nrf.getTxAddress() );
+    printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", nrf.getRxAddress() );
+}
+
+void initNRF(int mode, int msg_len)
+{
+#ifndef NRF_FIRST_INIT
+#define NRF_FIRST_INIT
+    nrf.setTxAddress(txAddr);
+    nrf.setRxAddress(rxAddr);
+    nrf.powerUp();
+#endif
+    switch(mode) {
+        case NRF_TRANSMIT:
+            nrf.setTransferSize(msg_len);
+            nrf.setTransmitMode();
+            nrf.enable();
+            break;
+        case NRF_RECEIVE:
+            nrf.setTransferSize(msg_len);
+            nrf.setReceiveMode();
+            nrf.enable();
+            break;
+        default:
+            printf("Invalid NRF Mode\r\n");
+    }
+}
+/*
+void getPayload(int id, unsigned int count, int lspeed, int rspeed, char* txData)
+{
+    *(txData+0) = '$';
+    *(txData+1) = id/10+'0';
+    *(txData+2) = id%10+'0';
+    *(txData+3) = count/10000+'0';
+    *(txData+4) = count/1000%10+'0';
+    *(txData+5) = count/100%10+'0';
+    *(txData+6) = count/10%10+'0';
+    *(txData+7) = count%10+'0';
+    *(txData+8) = lspeed>0?'+':'-';
+    *(txData+9) = abs(lspeed)/10+'0';
+    *(txData+10) = abs(lspeed)%10+'0';
+    *(txData+11) = rspeed>0?'+':'-';
+    *(txData+12) = abs(rspeed)/10+'0';
+    *(txData+13) = abs(rspeed)%10+'0';
+    *(txData+14) = '\0';
+}
+xxyyyyyabbcddn
+
+14 chars
+
+xx: robot_id
+yyyyy: packet_id
+a: sign of lspeed
+bb: lspeed
+c: sign of rspeed
+dd: rspeed
+n: NULL 0
+
+0100001+30+30
+*/
\ No newline at end of file