Hawk Eye
/
ROVReceiver
Receiver for rc ROV
Diff: main.cpp
- Revision:
- 1:e11cc38ef502
- Parent:
- 0:a8a417d50cc7
- Child:
- 2:ce397d92f68e
diff -r a8a417d50cc7 -r e11cc38ef502 main.cpp --- a/main.cpp Tue Aug 28 16:57:47 2018 +0000 +++ b/main.cpp Mon Sep 10 20:11:24 2018 +0000 @@ -4,29 +4,65 @@ #define CHANNELS_NUMBER 12 #define CMD_VALUE 65535 #define CMD_GET_STATUS 1 -//DigitalOut myled(LED1); +#define CMD_INITIALIZE 2 +#define RCV_TIMEOUT_MS 1000 #define SERVO_MAX 2500 #define SERVO_MIN 800 #define SERVO_CENTER SERVO_MAX-SERVO_MIN +#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; -//PwmOut v_servo(PTC2); +uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION; + +/* +PwmOut depthEsc1(PTC2); +PwmOut depthEsc2(PTC2); +PwmOut leftEsc(PTC2); +PwmOut rightEsc(PTC2); +*/ + +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)); } int main() { - //v_servo.period_us(20000); // servo requires a 20ms period - //v_servo.pulsewidth_us(SERVO_CENTER); + /* + depthEsc1.period_us(20000); // servo requires a 20ms period + depthEsc1.pulsewidth_us(SERVO_CENTER); + depthEsc2.period_us(20000); // servo requires a 20ms period + depthEsc2.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(); @@ -51,11 +87,24 @@ 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) { - while( !radio.readable() ); - { + 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) { @@ -63,18 +112,24 @@ switch(buffer[1]){ case CMD_GET_STATUS: zeroBuffer(); - strcpy((char*)buffer,"I'm alive"); + //strcpy((char*)buffer,"I'm alive"); + buffer[0]=state; + buffer[1]=t.read_us(); radio.setTransmitMode(); //wait(0.1); - int bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) ); + 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); + //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++){ @@ -83,6 +138,8 @@ pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]); } } + + } } \ No newline at end of file