Diff: slave.cpp

Branch:

RF24_library_test_tx

Revision:

15:07f67c448042

Child:

16:d9c8e60bfdb1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/slave.cpp	Sat Dec 01 19:45:24 2018 +0000
@@ -0,0 +1,209 @@
+#include "mbed.h"
+#include "nRF24L01P.h"
+#include "beep.h"
+
+#define PING 1
+#define PONG 2
+#define PC 3
+
+#define ROLE PONG
+#define ID 1
+
+#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
+
+nRF24L01P nrf(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE, spi_IRQ);    // mosi, miso, sck, csn, ce, irq
+
+Serial pc(USBTX, USBRX);
+Serial lidar(D1, D0);
+
+PwmOut motor_RA(D9);
+PwmOut motor_RB(D10);
+PwmOut motor_LA(D3);
+PwmOut motor_LB(D6);
+PwmOut led_B(A5);
+PwmOut led_G(A2);
+PwmOut led_R(A1);
+PwmOut buzzer(D5);
+AnalogIn batteryCheck(A0);
+
+void beepStart();
+void endBeep();
+void initNRF(int role);
+void dumpRFInfo();
+void turnWheel(int rspd, int lspd);
+void getPayload(int id, unsigned int count, int lspeed, int rspeed, char* txData);
+
+
+int main() {
+    int role = ROLE;
+    int id=0;
+    char txData[TRANSFER_SIZE];
+    int txDataCnt = 0;
+    char rxData[TRANSFER_SIZE];
+    int rxDataCnt = 0;
+    
+    
+    int inByteCnt = 0;
+    char buf[TRANSFER_SIZE+3];
+    
+    beepStart();
+    pc.baud(115200);
+    initNRF(role);
+//    dumpRFInfo();
+    
+    while(1) {
+        switch(role) {
+        case PING:
+            int id = 1;
+            int lspeed=-40;
+            int rspeed=50;
+            txDataCnt++;
+            getPayload(id, txDataCnt, lspeed, rspeed, txData);
+            pc.printf("PING:%s\r\n", txData);
+            nrf.write(NRF24L01P_PIPE_P0, txData, TRANSFER_SIZE);
+            wait(0.2);
+            break;
+        case PONG:
+            if ( nrf.readable() ) {
+                rxDataCnt = nrf.read( NRF24L01P_PIPE_P0, rxData, TRANSFER_SIZE);
+                if(rxData[0] =='$') {
+                    int id = (rxData[1]-'0')*10+(rxData[2]-'0');
+                    int dataCount = (rxData[3]-'0')*10000+(rxData[4]-'0')*1000+(rxData[5]-'0')*100+(rxData[6]-'0')*10+(rxData[7]-'0');
+                    int rspd = (rxData[9]-'0')*10+(rxData[10]-'0');
+                    int lspd = (rxData[12]-'0')*10+(rxData[13]-'0');
+                    if(rxData[8]=='-')
+                        rspd = -rspd;
+                    if(rxData[11]=='-')
+                        lspd = -lspd;
+                        
+                    if(true) {
+                        turnWheel(rspd, lspd);
+                        printf("[%d] RxData: %s, rspd: %d, rspd: %d\r\n", id, rxData, rspd, lspd);
+                        char buf[] = "JUSTDUMMYREPLY";
+                        wait(0.005);
+                        nrf.setTxAddress(0xDEADBEEF00);
+                        nrf.setTransferSize( TRANSFER_SIZE );
+                        nrf.setTransmitMode();
+                        nrf.enable();
+                        nrf.write(NRF24L01P_PIPE_P0, buf, TRANSFER_SIZE);
+                        
+                        
+                        nrf.setRxAddress(0xDEADBEEF0F);
+                        nrf.powerUp();
+                        nrf.setTransferSize( TRANSFER_SIZE );
+                        nrf.setReceiveMode();
+                        nrf.enable();
+                    }
+                } else {
+                    printf("Invalid Data\r\n");
+                }
+            }
+            break;
+        case PC:
+            while(pc.readable()) {
+                char inByte = pc.getc();
+                if(inByteCnt > TRANSFER_SIZE+2) {
+                    printf("Data size overflow!\r\n");
+                    inByteCnt = 0;
+                    break;
+                } else if(inByte == '\0') {
+                    buf[inByteCnt] = '\0';
+                    printf("Command: %s\r\n", buf);
+                    nrf.write(NRF24L01P_PIPE_P0, buf, TRANSFER_SIZE);
+                    inByteCnt=0;
+                } else {
+                    buf[inByteCnt++] = inByte;
+                }
+            }
+            break;
+        }
+    }
+}
+
+void turnWheel(int rspd, int lspd)
+{
+    if(rspd>0) {
+        motor_RA.write((float)rspd/100);
+        motor_RB = 0;
+    } else {
+        rspd=-rspd;
+        motor_RB.write((float)rspd/100);
+        motor_RA = 0;
+    }
+    if(lspd>0) {
+        motor_LA.write((float)lspd/100);
+        motor_LB = 0;
+    } else {
+        lspd = -lspd;
+        motor_LB.write((float)lspd/100);
+        motor_LA = 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 role)
+{
+    if(role == PING || role == PC) {
+        nrf.setTxAddress(0xDEADBEEF0F);
+        nrf.powerUp();
+        nrf.setTransferSize( TRANSFER_SIZE );
+        nrf.setTransmitMode();
+        nrf.enable();
+    } else {
+        nrf.setRxAddress(0xDEADBEEF0F);
+        nrf.powerUp();
+        nrf.setTransferSize( TRANSFER_SIZE );
+        nrf.setReceiveMode();
+        nrf.enable();
+    }
+}
+
+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