현성 김 / Mbed 2 deprecated 181202_Castone_design_slave

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed nRF24L01P

slave.cpp@15:07f67c448042, 2018-12-01 (annotated)

Committer:
hyunsungkim
Date:
Sat Dec 01 19:45:24 2018 +0000
Branch:
RF24_library_test_tx
Revision:
15:07f67c448042
Child:
16:d9c8e60bfdb1
??? ??? ???. ??? ??. ?? ?? ?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hyunsungkim 15:07f67c448042 1 #include "mbed.h"
hyunsungkim 15:07f67c448042 2 #include "nRF24L01P.h"
hyunsungkim 15:07f67c448042 3 #include "beep.h"
hyunsungkim 15:07f67c448042 4
hyunsungkim 15:07f67c448042 5 #define PING 1
hyunsungkim 15:07f67c448042 6 #define PONG 2
hyunsungkim 15:07f67c448042 7 #define PC 3
hyunsungkim 15:07f67c448042 8
hyunsungkim 15:07f67c448042 9 #define ROLE PONG
hyunsungkim 15:07f67c448042 10 #define ID 1
hyunsungkim 15:07f67c448042 11
hyunsungkim 15:07f67c448042 12 #define nrf_CE D2
hyunsungkim 15:07f67c448042 13 #define nrf_CSN A3
hyunsungkim 15:07f67c448042 14 #define spi_SCK D13
hyunsungkim 15:07f67c448042 15 #define spi_MOSI D11
hyunsungkim 15:07f67c448042 16 #define spi_MISO D12
hyunsungkim 15:07f67c448042 17 #define spi_IRQ D4
hyunsungkim 15:07f67c448042 18
hyunsungkim 15:07f67c448042 19 #define TRANSFER_SIZE 15
hyunsungkim 15:07f67c448042 20
hyunsungkim 15:07f67c448042 21 nRF24L01P nrf(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE, spi_IRQ); // mosi, miso, sck, csn, ce, irq
hyunsungkim 15:07f67c448042 22
hyunsungkim 15:07f67c448042 23 Serial pc(USBTX, USBRX);
hyunsungkim 15:07f67c448042 24 Serial lidar(D1, D0);
hyunsungkim 15:07f67c448042 25
hyunsungkim 15:07f67c448042 26 PwmOut motor_RA(D9);
hyunsungkim 15:07f67c448042 27 PwmOut motor_RB(D10);
hyunsungkim 15:07f67c448042 28 PwmOut motor_LA(D3);
hyunsungkim 15:07f67c448042 29 PwmOut motor_LB(D6);
hyunsungkim 15:07f67c448042 30 PwmOut led_B(A5);
hyunsungkim 15:07f67c448042 31 PwmOut led_G(A2);
hyunsungkim 15:07f67c448042 32 PwmOut led_R(A1);
hyunsungkim 15:07f67c448042 33 PwmOut buzzer(D5);
hyunsungkim 15:07f67c448042 34 AnalogIn batteryCheck(A0);
hyunsungkim 15:07f67c448042 35
hyunsungkim 15:07f67c448042 36 void beepStart();
hyunsungkim 15:07f67c448042 37 void endBeep();
hyunsungkim 15:07f67c448042 38 void initNRF(int role);
hyunsungkim 15:07f67c448042 39 void dumpRFInfo();
hyunsungkim 15:07f67c448042 40 void turnWheel(int rspd, int lspd);
hyunsungkim 15:07f67c448042 41 void getPayload(int id, unsigned int count, int lspeed, int rspeed, char* txData);
hyunsungkim 15:07f67c448042 42
hyunsungkim 15:07f67c448042 43
hyunsungkim 15:07f67c448042 44 int main() {
hyunsungkim 15:07f67c448042 45 int role = ROLE;
hyunsungkim 15:07f67c448042 46 int id=0;
hyunsungkim 15:07f67c448042 47 char txData[TRANSFER_SIZE];
hyunsungkim 15:07f67c448042 48 int txDataCnt = 0;
hyunsungkim 15:07f67c448042 49 char rxData[TRANSFER_SIZE];
hyunsungkim 15:07f67c448042 50 int rxDataCnt = 0;
hyunsungkim 15:07f67c448042 51
hyunsungkim 15:07f67c448042 52
hyunsungkim 15:07f67c448042 53 int inByteCnt = 0;
hyunsungkim 15:07f67c448042 54 char buf[TRANSFER_SIZE+3];
hyunsungkim 15:07f67c448042 55
hyunsungkim 15:07f67c448042 56 beepStart();
hyunsungkim 15:07f67c448042 57 pc.baud(115200);
hyunsungkim 15:07f67c448042 58 initNRF(role);
hyunsungkim 15:07f67c448042 59 // dumpRFInfo();
hyunsungkim 15:07f67c448042 60
hyunsungkim 15:07f67c448042 61 while(1) {
hyunsungkim 15:07f67c448042 62 switch(role) {
hyunsungkim 15:07f67c448042 63 case PING:
hyunsungkim 15:07f67c448042 64 int id = 1;
hyunsungkim 15:07f67c448042 65 int lspeed=-40;
hyunsungkim 15:07f67c448042 66 int rspeed=50;
hyunsungkim 15:07f67c448042 67 txDataCnt++;
hyunsungkim 15:07f67c448042 68 getPayload(id, txDataCnt, lspeed, rspeed, txData);
hyunsungkim 15:07f67c448042 69 pc.printf("PING:%s\r\n", txData);
hyunsungkim 15:07f67c448042 70 nrf.write(NRF24L01P_PIPE_P0, txData, TRANSFER_SIZE);
hyunsungkim 15:07f67c448042 71 wait(0.2);
hyunsungkim 15:07f67c448042 72 break;
hyunsungkim 15:07f67c448042 73 case PONG:
hyunsungkim 15:07f67c448042 74 if ( nrf.readable() ) {
hyunsungkim 15:07f67c448042 75 rxDataCnt = nrf.read( NRF24L01P_PIPE_P0, rxData, TRANSFER_SIZE);
hyunsungkim 15:07f67c448042 76 if(rxData[0] =='$') {
hyunsungkim 15:07f67c448042 77 int id = (rxData[1]-'0')*10+(rxData[2]-'0');
hyunsungkim 15:07f67c448042 78 int dataCount = (rxData[3]-'0')*10000+(rxData[4]-'0')*1000+(rxData[5]-'0')*100+(rxData[6]-'0')*10+(rxData[7]-'0');
hyunsungkim 15:07f67c448042 79 int rspd = (rxData[9]-'0')*10+(rxData[10]-'0');
hyunsungkim 15:07f67c448042 80 int lspd = (rxData[12]-'0')*10+(rxData[13]-'0');
hyunsungkim 15:07f67c448042 81 if(rxData[8]=='-')
hyunsungkim 15:07f67c448042 82 rspd = -rspd;
hyunsungkim 15:07f67c448042 83 if(rxData[11]=='-')
hyunsungkim 15:07f67c448042 84 lspd = -lspd;
hyunsungkim 15:07f67c448042 85
hyunsungkim 15:07f67c448042 86 if(true) {
hyunsungkim 15:07f67c448042 87 turnWheel(rspd, lspd);
hyunsungkim 15:07f67c448042 88 printf("[%d] RxData: %s, rspd: %d, rspd: %d\r\n", id, rxData, rspd, lspd);
hyunsungkim 15:07f67c448042 89 char buf[] = "JUSTDUMMYREPLY";
hyunsungkim 15:07f67c448042 90 wait(0.005);
hyunsungkim 15:07f67c448042 91 nrf.setTxAddress(0xDEADBEEF00);
hyunsungkim 15:07f67c448042 92 nrf.setTransferSize( TRANSFER_SIZE );
hyunsungkim 15:07f67c448042 93 nrf.setTransmitMode();
hyunsungkim 15:07f67c448042 94 nrf.enable();
hyunsungkim 15:07f67c448042 95 nrf.write(NRF24L01P_PIPE_P0, buf, TRANSFER_SIZE);
hyunsungkim 15:07f67c448042 96
hyunsungkim 15:07f67c448042 97
hyunsungkim 15:07f67c448042 98 nrf.setRxAddress(0xDEADBEEF0F);
hyunsungkim 15:07f67c448042 99 nrf.powerUp();
hyunsungkim 15:07f67c448042 100 nrf.setTransferSize( TRANSFER_SIZE );
hyunsungkim 15:07f67c448042 101 nrf.setReceiveMode();
hyunsungkim 15:07f67c448042 102 nrf.enable();
hyunsungkim 15:07f67c448042 103 }
hyunsungkim 15:07f67c448042 104 } else {
hyunsungkim 15:07f67c448042 105 printf("Invalid Data\r\n");
hyunsungkim 15:07f67c448042 106 }
hyunsungkim 15:07f67c448042 107 }
hyunsungkim 15:07f67c448042 108 break;
hyunsungkim 15:07f67c448042 109 case PC:
hyunsungkim 15:07f67c448042 110 while(pc.readable()) {
hyunsungkim 15:07f67c448042 111 char inByte = pc.getc();
hyunsungkim 15:07f67c448042 112 if(inByteCnt > TRANSFER_SIZE+2) {
hyunsungkim 15:07f67c448042 113 printf("Data size overflow!\r\n");
hyunsungkim 15:07f67c448042 114 inByteCnt = 0;
hyunsungkim 15:07f67c448042 115 break;
hyunsungkim 15:07f67c448042 116 } else if(inByte == '\0') {
hyunsungkim 15:07f67c448042 117 buf[inByteCnt] = '\0';
hyunsungkim 15:07f67c448042 118 printf("Command: %s\r\n", buf);
hyunsungkim 15:07f67c448042 119 nrf.write(NRF24L01P_PIPE_P0, buf, TRANSFER_SIZE);
hyunsungkim 15:07f67c448042 120 inByteCnt=0;
hyunsungkim 15:07f67c448042 121 } else {
hyunsungkim 15:07f67c448042 122 buf[inByteCnt++] = inByte;
hyunsungkim 15:07f67c448042 123 }
hyunsungkim 15:07f67c448042 124 }
hyunsungkim 15:07f67c448042 125 break;
hyunsungkim 15:07f67c448042 126 }
hyunsungkim 15:07f67c448042 127 }
hyunsungkim 15:07f67c448042 128 }
hyunsungkim 15:07f67c448042 129
hyunsungkim 15:07f67c448042 130 void turnWheel(int rspd, int lspd)
hyunsungkim 15:07f67c448042 131 {
hyunsungkim 15:07f67c448042 132 if(rspd>0) {
hyunsungkim 15:07f67c448042 133 motor_RA.write((float)rspd/100);
hyunsungkim 15:07f67c448042 134 motor_RB = 0;
hyunsungkim 15:07f67c448042 135 } else {
hyunsungkim 15:07f67c448042 136 rspd=-rspd;
hyunsungkim 15:07f67c448042 137 motor_RB.write((float)rspd/100);
hyunsungkim 15:07f67c448042 138 motor_RA = 0;
hyunsungkim 15:07f67c448042 139 }
hyunsungkim 15:07f67c448042 140 if(lspd>0) {
hyunsungkim 15:07f67c448042 141 motor_LA.write((float)lspd/100);
hyunsungkim 15:07f67c448042 142 motor_LB = 0;
hyunsungkim 15:07f67c448042 143 } else {
hyunsungkim 15:07f67c448042 144 lspd = -lspd;
hyunsungkim 15:07f67c448042 145 motor_LB.write((float)lspd/100);
hyunsungkim 15:07f67c448042 146 motor_LA = 0;
hyunsungkim 15:07f67c448042 147 }
hyunsungkim 15:07f67c448042 148 }
hyunsungkim 15:07f67c448042 149
hyunsungkim 15:07f67c448042 150 void dumpRFInfo()
hyunsungkim 15:07f67c448042 151 {
hyunsungkim 15:07f67c448042 152 printf( "nRF24L01+ Frequency : %d MHz\r\n", nrf.getRfFrequency() );
hyunsungkim 15:07f67c448042 153 printf( "nRF24L01+ Output power : %d dBm\r\n", nrf.getRfOutputPower() );
hyunsungkim 15:07f67c448042 154 printf( "nRF24L01+ Data Rate : %d kbps\r\n", nrf.getAirDataRate() );
hyunsungkim 15:07f67c448042 155 printf( "nRF24L01+ TX Address : 0x%010llX\r\n", nrf.getTxAddress() );
hyunsungkim 15:07f67c448042 156 printf( "nRF24L01+ RX Address : 0x%010llX\r\n", nrf.getRxAddress() );
hyunsungkim 15:07f67c448042 157 }
hyunsungkim 15:07f67c448042 158
hyunsungkim 15:07f67c448042 159 void initNRF(int role)
hyunsungkim 15:07f67c448042 160 {
hyunsungkim 15:07f67c448042 161 if(role == PING || role == PC) {
hyunsungkim 15:07f67c448042 162 nrf.setTxAddress(0xDEADBEEF0F);
hyunsungkim 15:07f67c448042 163 nrf.powerUp();
hyunsungkim 15:07f67c448042 164 nrf.setTransferSize( TRANSFER_SIZE );
hyunsungkim 15:07f67c448042 165 nrf.setTransmitMode();
hyunsungkim 15:07f67c448042 166 nrf.enable();
hyunsungkim 15:07f67c448042 167 } else {
hyunsungkim 15:07f67c448042 168 nrf.setRxAddress(0xDEADBEEF0F);
hyunsungkim 15:07f67c448042 169 nrf.powerUp();
hyunsungkim 15:07f67c448042 170 nrf.setTransferSize( TRANSFER_SIZE );
hyunsungkim 15:07f67c448042 171 nrf.setReceiveMode();
hyunsungkim 15:07f67c448042 172 nrf.enable();
hyunsungkim 15:07f67c448042 173 }
hyunsungkim 15:07f67c448042 174 }
hyunsungkim 15:07f67c448042 175
hyunsungkim 15:07f67c448042 176 void getPayload(int id, unsigned int count, int lspeed, int rspeed, char* txData)
hyunsungkim 15:07f67c448042 177 {
hyunsungkim 15:07f67c448042 178 *(txData+0) = '$';
hyunsungkim 15:07f67c448042 179 *(txData+1) = id/10+'0';
hyunsungkim 15:07f67c448042 180 *(txData+2) = id%10+'0';
hyunsungkim 15:07f67c448042 181 *(txData+3) = count/10000+'0';
hyunsungkim 15:07f67c448042 182 *(txData+4) = count/1000%10+'0';
hyunsungkim 15:07f67c448042 183 *(txData+5) = count/100%10+'0';
hyunsungkim 15:07f67c448042 184 *(txData+6) = count/10%10+'0';
hyunsungkim 15:07f67c448042 185 *(txData+7) = count%10+'0';
hyunsungkim 15:07f67c448042 186 *(txData+8) = lspeed>0?'+':'-';
hyunsungkim 15:07f67c448042 187 *(txData+9) = abs(lspeed)/10+'0';
hyunsungkim 15:07f67c448042 188 *(txData+10) = abs(lspeed)%10+'0';
hyunsungkim 15:07f67c448042 189 *(txData+11) = rspeed>0?'+':'-';
hyunsungkim 15:07f67c448042 190 *(txData+12) = abs(rspeed)/10+'0';
hyunsungkim 15:07f67c448042 191 *(txData+13) = abs(rspeed)%10+'0';
hyunsungkim 15:07f67c448042 192 *(txData+14) = '\0';
hyunsungkim 15:07f67c448042 193 }
hyunsungkim 15:07f67c448042 194
hyunsungkim 15:07f67c448042 195 /*
hyunsungkim 15:07f67c448042 196 xxyyyyyabbcddn
hyunsungkim 15:07f67c448042 197
hyunsungkim 15:07f67c448042 198 14 chars
hyunsungkim 15:07f67c448042 199
hyunsungkim 15:07f67c448042 200 xx: robot_id
hyunsungkim 15:07f67c448042 201 yyyyy: packet_id
hyunsungkim 15:07f67c448042 202 a: sign of lspeed
hyunsungkim 15:07f67c448042 203 bb: lspeed
hyunsungkim 15:07f67c448042 204 c: sign of rspeed
hyunsungkim 15:07f67c448042 205 dd: rspeed
hyunsungkim 15:07f67c448042 206 n: NULL 0
hyunsungkim 15:07f67c448042 207
hyunsungkim 15:07f67c448042 208 0100001+30+30
hyunsungkim 15:07f67c448042 209 */