Hawk Eye / Mbed 2 deprecated ROVReceiver

Receiver for rc ROV

Dependencies:   mbed nRF24L01P

main.cpp@2:ce397d92f68e, 2018-09-23 (annotated)

Committer:
spin7ion
Date:
Sun Sep 23 12:52:12 2018 +0000
Revision:
2:ce397d92f68e
Parent:
1:e11cc38ef502 
First sail

Who changed what in which revision?

UserRevisionLine numberNew contents of line
spin7ion 0:a8a417d50cc7 1 #include "mbed.h"
spin7ion 0:a8a417d50cc7 2 #include "nRF24L01P.h"
spin7ion 0:a8a417d50cc7 3
spin7ion 0:a8a417d50cc7 4 #define CMD_VALUE 65535
spin7ion 0:a8a417d50cc7 5 #define CMD_GET_STATUS 1
spin7ion 1:e11cc38ef502 6 #define CMD_INITIALIZE 2
spin7ion 0:a8a417d50cc7 7
spin7ion 1:e11cc38ef502 8 #define RCV_TIMEOUT_MS 1000
spin7ion 0:a8a417d50cc7 9
spin7ion 2:ce397d92f68e 10 #define SERVO_MAX 2200
spin7ion 0:a8a417d50cc7 11 #define SERVO_MIN 800
spin7ion 2:ce397d92f68e 12 #define SERVO_CENTER (SERVO_MAX-SERVO_MIN)/2+SERVO_MIN
spin7ion 2:ce397d92f68e 13
spin7ion 2:ce397d92f68e 14 #define CHANNELS_NUMBER 12
spin7ion 2:ce397d92f68e 15 #define CHANNEL_MAX 1024
spin7ion 2:ce397d92f68e 16 #define CHANNEL_MIN 0
spin7ion 2:ce397d92f68e 17 #define CHANNEL_DEPTH 3
spin7ion 2:ce397d92f68e 18 #define CHANNEL_YAW 1
spin7ion 2:ce397d92f68e 19 #define CHANNEL_THROTTLE 0
spin7ion 0:a8a417d50cc7 20
spin7ion 1:e11cc38ef502 21 #define STATE_INITILIZATION 0x00
spin7ion 1:e11cc38ef502 22 #define STATE_RUN 0x01
spin7ion 1:e11cc38ef502 23 #define STATE_FAILSAFE 0x02
spin7ion 1:e11cc38ef502 24
spin7ion 0:a8a417d50cc7 25 Serial pc(USBTX, USBRX); // tx, rx
spin7ion 0:a8a417d50cc7 26
spin7ion 0:a8a417d50cc7 27 nRF24L01P radio(D11, D12, D13, D10, D9, D8); // mosi, miso, sck, csn, ce, irq
spin7ion 0:a8a417d50cc7 28
spin7ion 0:a8a417d50cc7 29 uint16_t channels[CHANNELS_NUMBER];
spin7ion 0:a8a417d50cc7 30 uint16_t buffer [CHANNELS_NUMBER];
spin7ion 1:e11cc38ef502 31 int bytesWritten;
spin7ion 0:a8a417d50cc7 32
spin7ion 1:e11cc38ef502 33 uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION;
spin7ion 1:e11cc38ef502 34
spin7ion 2:ce397d92f68e 35
spin7ion 2:ce397d92f68e 36 PwmOut depthEsc(PA_10);
spin7ion 2:ce397d92f68e 37 PwmOut leftEsc(PA_8);
spin7ion 2:ce397d92f68e 38 PwmOut rightEsc(PA_11);
spin7ion 2:ce397d92f68e 39
spin7ion 1:e11cc38ef502 40
spin7ion 1:e11cc38ef502 41 Timer t;
spin7ion 1:e11cc38ef502 42
spin7ion 1:e11cc38ef502 43 void setState(uint8_t newState) {
spin7ion 1:e11cc38ef502 44 state=state | newState;
spin7ion 1:e11cc38ef502 45 }
spin7ion 1:e11cc38ef502 46
spin7ion 1:e11cc38ef502 47 void initialize() {
spin7ion 1:e11cc38ef502 48 state=((uint16_t)0x01 << 8) | state;
spin7ion 1:e11cc38ef502 49 }
spin7ion 1:e11cc38ef502 50
spin7ion 1:e11cc38ef502 51 bool isInitializated() {
spin7ion 1:e11cc38ef502 52 return (state>>8) & 0x01;
spin7ion 1:e11cc38ef502 53 }
spin7ion 0:a8a417d50cc7 54
spin7ion 0:a8a417d50cc7 55 void zeroBuffer(){
spin7ion 0:a8a417d50cc7 56 memset(buffer, 0, sizeof(buffer));
spin7ion 0:a8a417d50cc7 57 }
spin7ion 0:a8a417d50cc7 58
spin7ion 2:ce397d92f68e 59 float normToPWM(float chNorm){
spin7ion 2:ce397d92f68e 60 return chNorm*(SERVO_MAX-SERVO_MIN)+SERVO_MIN;
spin7ion 2:ce397d92f68e 61 }
spin7ion 2:ce397d92f68e 62 float normChannel(uint16_t channel){
spin7ion 2:ce397d92f68e 63 return ((float)channel-CHANNEL_MIN)/(CHANNEL_MAX-CHANNEL_MIN);
spin7ion 2:ce397d92f68e 64 }
spin7ion 2:ce397d92f68e 65 float chToPWM(uint16_t channel){
spin7ion 2:ce397d92f68e 66 return normToPWM(normChannel(channel));
spin7ion 2:ce397d92f68e 67 }
spin7ion 2:ce397d92f68e 68
spin7ion 2:ce397d92f68e 69 void channelsToEsc() {
spin7ion 2:ce397d92f68e 70 if (!isInitializated()) {
spin7ion 2:ce397d92f68e 71 pc.printf("WARNING!!!! NOT INITIALIZATED!!!!\r\n");
spin7ion 2:ce397d92f68e 72 return;
spin7ion 2:ce397d92f68e 73 }
spin7ion 2:ce397d92f68e 74 depthEsc.pulsewidth_us(chToPWM(channels[CHANNEL_DEPTH])); // servo position determined by a pulsewidth between 1-2ms
spin7ion 2:ce397d92f68e 75
spin7ion 2:ce397d92f68e 76 float forwardFactor = (1.0f-normChannel(channels[CHANNEL_THROTTLE]))-0.5f;
spin7ion 2:ce397d92f68e 77 float yawFactor = normChannel(channels[CHANNEL_YAW])-0.5f;
spin7ion 2:ce397d92f68e 78 pc.printf("\t\t\t\tff: %f; yf: %f\r\n", forwardFactor, yawFactor);
spin7ion 2:ce397d92f68e 79 leftEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500+yawFactor*500);
spin7ion 2:ce397d92f68e 80 rightEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500-yawFactor*500);
spin7ion 2:ce397d92f68e 81 }
spin7ion 2:ce397d92f68e 82
spin7ion 0:a8a417d50cc7 83 int main() {
spin7ion 2:ce397d92f68e 84
spin7ion 2:ce397d92f68e 85 depthEsc.period_us(20000); // servo requires a 20ms period
spin7ion 2:ce397d92f68e 86 depthEsc.pulsewidth_us(SERVO_CENTER);
spin7ion 1:e11cc38ef502 87 leftEsc.period_us(20000); // servo requires a 20ms period
spin7ion 1:e11cc38ef502 88 leftEsc.pulsewidth_us(SERVO_CENTER);
spin7ion 1:e11cc38ef502 89 rightEsc.period_us(20000); // servo requires a 20ms period
spin7ion 1:e11cc38ef502 90 rightEsc.pulsewidth_us(SERVO_CENTER);
spin7ion 1:e11cc38ef502 91
spin7ion 2:ce397d92f68e 92
spin7ion 0:a8a417d50cc7 93 //v_servo.pulsewidth_us(pulse); // servo position determined by a pulsewidth between 1-2ms
spin7ion 0:a8a417d50cc7 94
spin7ion 2:ce397d92f68e 95 //radio.powerUp();
spin7ion 0:a8a417d50cc7 96 pc.baud(115200);
spin7ion 0:a8a417d50cc7 97 pc.printf("We all live in a yellow submarine!\r\n");
spin7ion 0:a8a417d50cc7 98
spin7ion 0:a8a417d50cc7 99 radio.setRxAddress(0xDEADBEEF0F);
spin7ion 0:a8a417d50cc7 100 radio.setTxAddress(0xDEADC0DE0F);
spin7ion 0:a8a417d50cc7 101 radio.setRfFrequency(2405);
spin7ion 0:a8a417d50cc7 102 radio.powerUp();
spin7ion 0:a8a417d50cc7 103
spin7ion 0:a8a417d50cc7 104 pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", radio.getRfFrequency() );
spin7ion 0:a8a417d50cc7 105 pc.printf( "nRF24L01+ Output power : %d dBm\r\n", radio.getRfOutputPower() );
spin7ion 0:a8a417d50cc7 106 pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", radio.getAirDataRate() );
spin7ion 0:a8a417d50cc7 107 pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", radio.getTxAddress() );
spin7ion 0:a8a417d50cc7 108 pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", radio.getRxAddress() );
spin7ion 2:ce397d92f68e 109
spin7ion 2:ce397d92f68e 110 /*while (radio.getRxAddress()!=0xDEADBEEF0F){
spin7ion 2:ce397d92f68e 111 pc.printf("RX address fail!\r\n");
spin7ion 2:ce397d92f68e 112 radio.powerDown();
spin7ion 2:ce397d92f68e 113 radio.setRxAddress(0xDEADBEEF0F);
spin7ion 2:ce397d92f68e 114 radio.powerUp();
spin7ion 2:ce397d92f68e 115 }*/
spin7ion 0:a8a417d50cc7 116
spin7ion 0:a8a417d50cc7 117 radio.setTransferSize( CHANNELS_NUMBER*sizeof(uint16_t) );
spin7ion 0:a8a417d50cc7 118
spin7ion 0:a8a417d50cc7 119 radio.setReceiveMode();
spin7ion 0:a8a417d50cc7 120
spin7ion 0:a8a417d50cc7 121 radio.enable();
spin7ion 0:a8a417d50cc7 122
spin7ion 0:a8a417d50cc7 123 pc.printf("Greetings, capitain!\r\n");
spin7ion 1:e11cc38ef502 124 pc.printf("is initializated: %s\r\n", isInitializated()?"true":"false");
spin7ion 1:e11cc38ef502 125 pc.printf( "0x%X\r\n", state );
spin7ion 1:e11cc38ef502 126 t.start();
spin7ion 0:a8a417d50cc7 127
spin7ion 0:a8a417d50cc7 128 while (1)
spin7ion 0:a8a417d50cc7 129 {
spin7ion 1:e11cc38ef502 130 int startTime=t.read_ms();
spin7ion 1:e11cc38ef502 131 while( !radio.readable() && t.read_ms()-startTime < RCV_TIMEOUT_MS);
spin7ion 1:e11cc38ef502 132
spin7ion 1:e11cc38ef502 133 if(!radio.readable()) {
spin7ion 1:e11cc38ef502 134 //failsafe
spin7ion 1:e11cc38ef502 135 setState(STATE_FAILSAFE);
spin7ion 1:e11cc38ef502 136 pc.printf("Failsafe\n\r", channels[0],channels[1],channels[2],channels[3]);
spin7ion 1:e11cc38ef502 137 channels[0]=512;
spin7ion 1:e11cc38ef502 138 channels[1]=512;
spin7ion 1:e11cc38ef502 139 channels[2]=512;
spin7ion 1:e11cc38ef502 140 channels[3]=512;
spin7ion 1:e11cc38ef502 141 } else {
spin7ion 0:a8a417d50cc7 142 int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
spin7ion 0:a8a417d50cc7 143
spin7ion 0:a8a417d50cc7 144 if (buffer[0] == CMD_VALUE) {
spin7ion 0:a8a417d50cc7 145 pc.printf("Command: %d\r\n", buffer[1]);
spin7ion 0:a8a417d50cc7 146 switch(buffer[1]){
spin7ion 0:a8a417d50cc7 147 case CMD_GET_STATUS:
spin7ion 0:a8a417d50cc7 148 zeroBuffer();
spin7ion 1:e11cc38ef502 149 //strcpy((char*)buffer,"I'm alive");
spin7ion 1:e11cc38ef502 150 buffer[0]=state;
spin7ion 1:e11cc38ef502 151 buffer[1]=t.read_us();
spin7ion 0:a8a417d50cc7 152 radio.setTransmitMode();
spin7ion 0:a8a417d50cc7 153 //wait(0.1);
spin7ion 1:e11cc38ef502 154 bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
spin7ion 0:a8a417d50cc7 155 if (bytesWritten<2*CHANNELS_NUMBER){
spin7ion 0:a8a417d50cc7 156 pc.printf("Transmit error\r\n");
spin7ion 0:a8a417d50cc7 157 } else {
spin7ion 1:e11cc38ef502 158 //pc.printf("Sent telemetry: %s\r\n",(char*)buffer);
spin7ion 0:a8a417d50cc7 159 }
spin7ion 0:a8a417d50cc7 160 radio.setReceiveMode();
spin7ion 0:a8a417d50cc7 161 //wait(0.1);
spin7ion 0:a8a417d50cc7 162 break;
spin7ion 1:e11cc38ef502 163 case CMD_INITIALIZE:
spin7ion 1:e11cc38ef502 164 initialize();
spin7ion 1:e11cc38ef502 165 setState(STATE_RUN);
spin7ion 1:e11cc38ef502 166 break;
spin7ion 0:a8a417d50cc7 167 }
spin7ion 0:a8a417d50cc7 168 } else {
spin7ion 0:a8a417d50cc7 169 for(int i=0; i<CHANNELS_NUMBER; i++){
spin7ion 0:a8a417d50cc7 170 channels[i] = buffer[i];
spin7ion 0:a8a417d50cc7 171 }
spin7ion 2:ce397d92f68e 172 channelsToEsc();
spin7ion 0:a8a417d50cc7 173 pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]);
spin7ion 0:a8a417d50cc7 174 }
spin7ion 0:a8a417d50cc7 175 }
spin7ion 1:e11cc38ef502 176
spin7ion 1:e11cc38ef502 177
spin7ion 0:a8a417d50cc7 178 }
spin7ion 0:a8a417d50cc7 179
spin7ion 0:a8a417d50cc7 180 }