File content as of revision 15:07f67c448042:

#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
*/