![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Receiver for rc ROV
main.cpp
- Committer:
- spin7ion
- Date:
- 2018-09-23
- Revision:
- 2:ce397d92f68e
- Parent:
- 1:e11cc38ef502
File content as of revision 2:ce397d92f68e:
#include "mbed.h" #include "nRF24L01P.h" #define CMD_VALUE 65535 #define CMD_GET_STATUS 1 #define CMD_INITIALIZE 2 #define RCV_TIMEOUT_MS 1000 #define SERVO_MAX 2200 #define SERVO_MIN 800 #define SERVO_CENTER (SERVO_MAX-SERVO_MIN)/2+SERVO_MIN #define CHANNELS_NUMBER 12 #define CHANNEL_MAX 1024 #define CHANNEL_MIN 0 #define CHANNEL_DEPTH 3 #define CHANNEL_YAW 1 #define CHANNEL_THROTTLE 0 #define STATE_INITILIZATION 0x00 #define STATE_RUN 0x01 #define STATE_FAILSAFE 0x02 Serial pc(USBTX, USBRX); // tx, rx nRF24L01P radio(D11, D12, D13, D10, D9, D8); // mosi, miso, sck, csn, ce, irq uint16_t channels[CHANNELS_NUMBER]; uint16_t buffer [CHANNELS_NUMBER]; int bytesWritten; uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION; PwmOut depthEsc(PA_10); PwmOut leftEsc(PA_8); PwmOut rightEsc(PA_11); Timer t; void setState(uint8_t newState) { state=state | newState; } void initialize() { state=((uint16_t)0x01 << 8) | state; } bool isInitializated() { return (state>>8) & 0x01; } void zeroBuffer(){ memset(buffer, 0, sizeof(buffer)); } float normToPWM(float chNorm){ return chNorm*(SERVO_MAX-SERVO_MIN)+SERVO_MIN; } float normChannel(uint16_t channel){ return ((float)channel-CHANNEL_MIN)/(CHANNEL_MAX-CHANNEL_MIN); } float chToPWM(uint16_t channel){ return normToPWM(normChannel(channel)); } void channelsToEsc() { if (!isInitializated()) { pc.printf("WARNING!!!! NOT INITIALIZATED!!!!\r\n"); return; } depthEsc.pulsewidth_us(chToPWM(channels[CHANNEL_DEPTH])); // servo position determined by a pulsewidth between 1-2ms float forwardFactor = (1.0f-normChannel(channels[CHANNEL_THROTTLE]))-0.5f; float yawFactor = normChannel(channels[CHANNEL_YAW])-0.5f; pc.printf("\t\t\t\tff: %f; yf: %f\r\n", forwardFactor, yawFactor); leftEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500+yawFactor*500); rightEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500-yawFactor*500); } int main() { depthEsc.period_us(20000); // servo requires a 20ms period depthEsc.pulsewidth_us(SERVO_CENTER); leftEsc.period_us(20000); // servo requires a 20ms period leftEsc.pulsewidth_us(SERVO_CENTER); rightEsc.period_us(20000); // servo requires a 20ms period rightEsc.pulsewidth_us(SERVO_CENTER); //v_servo.pulsewidth_us(pulse); // servo position determined by a pulsewidth between 1-2ms //radio.powerUp(); pc.baud(115200); pc.printf("We all live in a yellow submarine!\r\n"); radio.setRxAddress(0xDEADBEEF0F); radio.setTxAddress(0xDEADC0DE0F); radio.setRfFrequency(2405); radio.powerUp(); pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", radio.getRfFrequency() ); pc.printf( "nRF24L01+ Output power : %d dBm\r\n", radio.getRfOutputPower() ); pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", radio.getAirDataRate() ); pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", radio.getTxAddress() ); pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", radio.getRxAddress() ); /*while (radio.getRxAddress()!=0xDEADBEEF0F){ pc.printf("RX address fail!\r\n"); radio.powerDown(); radio.setRxAddress(0xDEADBEEF0F); radio.powerUp(); }*/ radio.setTransferSize( CHANNELS_NUMBER*sizeof(uint16_t) ); radio.setReceiveMode(); radio.enable(); pc.printf("Greetings, capitain!\r\n"); pc.printf("is initializated: %s\r\n", isInitializated()?"true":"false"); pc.printf( "0x%X\r\n", state ); t.start(); while (1) { int startTime=t.read_ms(); while( !radio.readable() && t.read_ms()-startTime < RCV_TIMEOUT_MS); if(!radio.readable()) { //failsafe setState(STATE_FAILSAFE); pc.printf("Failsafe\n\r", channels[0],channels[1],channels[2],channels[3]); channels[0]=512; channels[1]=512; channels[2]=512; channels[3]=512; } else { int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) ); if (buffer[0] == CMD_VALUE) { pc.printf("Command: %d\r\n", buffer[1]); switch(buffer[1]){ case CMD_GET_STATUS: zeroBuffer(); //strcpy((char*)buffer,"I'm alive"); buffer[0]=state; buffer[1]=t.read_us(); radio.setTransmitMode(); //wait(0.1); bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) ); if (bytesWritten<2*CHANNELS_NUMBER){ pc.printf("Transmit error\r\n"); } else { //pc.printf("Sent telemetry: %s\r\n",(char*)buffer); } radio.setReceiveMode(); //wait(0.1); break; case CMD_INITIALIZE: initialize(); setState(STATE_RUN); break; } } else { for(int i=0; i<CHANNELS_NUMBER; i++){ channels[i] = buffer[i]; } channelsToEsc(); pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]); } } } }