slave.cpp

00001 #include "mbed.h"
00002 #include "nRF24L01P.h"
00003 #include "beep.h"
00004 
00005 #define nrf_CE      D2
00006 #define nrf_CSN     A3
00007 #define spi_SCK     D13
00008 #define spi_MOSI    D11
00009 #define spi_MISO    D12
00010 #define spi_IRQ     D4
00011 
00012 #define NRF_TRANSMIT    1
00013 #define NRF_RECEIVE     2
00014 
00015 #define NRF_ID  7
00016 
00017 #define TRANSFER_SIZE 15
00018 
00019 nRF24L01P nrf(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE, spi_IRQ);    // mosi, miso, sck, csn, ce, irq
00020 
00021 uint64_t rxAddr=0xDEADBEEF0F;
00022 uint64_t txAddr=0xDEADBEEF00;
00023 
00024 Serial pc(USBTX, USBRX);
00025 //Serial lidar(D1, D0);
00026 
00027 PwmOut motor_RA(D9);
00028 PwmOut motor_RB(D10);
00029 PwmOut motor_LA(D3);
00030 PwmOut motor_LB(D6);
00031 PwmOut led_B(A5);
00032 PwmOut led_G(A2);
00033 PwmOut led_R(A1);
00034 PwmOut buzzer(D5);
00035 AnalogIn batteryCheck(A0);
00036 
00037 void beepStart();
00038 void dumpRFInfo();
00039 void turnWheel(int rspd, int lspd);
00040 void initNRF(int mode, int msg_len);
00041 
00042 int main()
00043 {
00044     char rxData[TRANSFER_SIZE];
00045     int rxDataCnt = 0;
00046 
00047     initNRF(NRF_RECEIVE, TRANSFER_SIZE);
00048     int inByteCnt = 0;
00049 
00050     beepStart();
00051     pc.baud(115200);
00052 //    dumpRFInfo();
00053     led_G = 1;
00054     led_R = 1;
00055 
00056     while(1) {
00057         if ( nrf.readable() ) {
00058             rxDataCnt = nrf.read( NRF24L01P_PIPE_P0, rxData, TRANSFER_SIZE);
00059             if(rxData[0] =='$') {
00060                 int id = (rxData[1]-'0')*10+(rxData[2]-'0');
00061                 int dataCount = (rxData[3]-'0')*10000+(rxData[4]-'0')*1000+(rxData[5]-'0')*100+(rxData[6]-'0')*10+(rxData[7]-'0');
00062                 int rspd = (rxData[9]-'0')*10+(rxData[10]-'0');
00063                 int lspd = (rxData[12]-'0')*10+(rxData[13]-'0');
00064                 if(rxData[8]=='-')
00065                     rspd = -rspd;
00066                 if(rxData[11]=='-')
00067                     lspd = -lspd;
00068 
00069                 if(id==NRF_ID) {
00070                     turnWheel(rspd, lspd);
00071                     printf("[%d] RxData: %s, rspd: %d, rspd: %d\r\n", id, rxData, rspd, lspd);
00072                     char buf[] = "JUSTDUMMYREPLY";
00073                     wait(0.005);
00074                     nrf.setTxAddress(0xDEADBEEF00);
00075                     nrf.setTransferSize( TRANSFER_SIZE );
00076                     nrf.setTransmitMode();
00077                     nrf.enable();
00078                     nrf.write(NRF24L01P_PIPE_P0, buf, TRANSFER_SIZE);
00079 
00080                     nrf.setRxAddress(0xDEADBEEF0F);
00081                     nrf.powerUp();
00082                     nrf.setTransferSize( TRANSFER_SIZE );
00083                     nrf.setReceiveMode();
00084                     nrf.enable();
00085                 }
00086             } else {
00087                 printf("Invalid Data\r\n");
00088             }
00089         }
00090     }
00091 }
00092 
00093 
00094 void turnWheel(int rspd, int lspd)
00095 {
00096     if(rspd>0) {
00097         motor_RA.write((float)rspd/100);
00098         motor_RB = 0;
00099     } else {
00100         rspd=-rspd;
00101         motor_RB.write((float)rspd/100);
00102         motor_RA = 0;
00103     }
00104     if(lspd>0) {
00105         motor_LA.write((float)lspd/100);
00106         motor_LB = 0;
00107     } else {
00108         lspd = -lspd;
00109         motor_LB.write((float)lspd/100);
00110         motor_LA = 0;
00111     }
00112 }
00113 
00114 void dumpRFInfo()
00115 {
00116     printf( "nRF24L01+ Frequency    : %d MHz\r\n",  nrf.getRfFrequency() );
00117     printf( "nRF24L01+ Output power : %d dBm\r\n",  nrf.getRfOutputPower() );
00118     printf( "nRF24L01+ Data Rate    : %d kbps\r\n", nrf.getAirDataRate() );
00119     printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", nrf.getTxAddress() );
00120     printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", nrf.getRxAddress() );
00121 }
00122 
00123 void initNRF(int mode, int msg_len)
00124 {
00125 #ifndef NRF_FIRST_INIT
00126 #define NRF_FIRST_INIT
00127     nrf.setTxAddress(txAddr);
00128     nrf.setRxAddress(rxAddr);
00129     nrf.powerUp();
00130 #endif
00131     switch(mode) {
00132         case NRF_TRANSMIT:
00133             nrf.setTransferSize(msg_len);
00134             nrf.setTransmitMode();
00135             nrf.enable();
00136             break;
00137         case NRF_RECEIVE:
00138             nrf.setTransferSize(msg_len);
00139             nrf.setReceiveMode();
00140             nrf.enable();
00141             break;
00142         default:
00143             printf("Invalid NRF Mode\r\n");
00144     }
00145 }
00146 
00147 
00148 /*
00149 xxyyyyyabbcddn
00150 
00151 14 chars
00152 
00153 xx: robot_id
00154 yyyyy: packet_id
00155 a: sign of lspeed
00156 bb: lspeed
00157 c: sign of rspeed
00158 dd: rspeed
00159 n: NULL 0
00160 
00161 0100001+30+30
00162 */