main.cpp

00001 #include "mbed.h"
00002 #include "nRF24L01P.h"
00003 
00004 #define CMD_VALUE 65535
00005 #define CMD_GET_STATUS 1
00006 #define CMD_INITIALIZE 2
00007 
00008 #define RCV_TIMEOUT_MS 1000
00009 
00010 #define SERVO_MAX 2200
00011 #define SERVO_MIN 800
00012 #define SERVO_CENTER (SERVO_MAX-SERVO_MIN)/2+SERVO_MIN
00013 
00014 #define CHANNELS_NUMBER 12 
00015 #define CHANNEL_MAX 1024
00016 #define CHANNEL_MIN 0
00017 #define CHANNEL_DEPTH       3
00018 #define CHANNEL_YAW         1
00019 #define CHANNEL_THROTTLE    0
00020 
00021 #define STATE_INITILIZATION 0x00
00022 #define STATE_RUN 0x01
00023 #define STATE_FAILSAFE 0x02
00024 
00025 Serial pc(USBTX, USBRX); // tx, rx
00026  
00027 nRF24L01P radio(D11, D12, D13, D10, D9, D8);    // mosi, miso, sck, csn, ce, irq
00028 
00029 uint16_t channels[CHANNELS_NUMBER];
00030 uint16_t buffer  [CHANNELS_NUMBER]; 
00031 int bytesWritten;
00032 
00033 uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION;
00034 
00035 
00036 PwmOut depthEsc(PA_10);
00037 PwmOut leftEsc(PA_8);
00038 PwmOut rightEsc(PA_11);
00039 
00040 
00041 Timer t;
00042 
00043 void setState(uint8_t newState) {
00044     state=state | newState;
00045 }
00046 
00047 void initialize() {
00048     state=((uint16_t)0x01 << 8) | state;
00049 }
00050 
00051 bool isInitializated() {
00052     return (state>>8) & 0x01;
00053 }
00054 
00055 void zeroBuffer(){
00056     memset(buffer, 0, sizeof(buffer));     
00057 }
00058 
00059 float normToPWM(float chNorm){
00060     return chNorm*(SERVO_MAX-SERVO_MIN)+SERVO_MIN;
00061 }
00062 float normChannel(uint16_t channel){
00063     return ((float)channel-CHANNEL_MIN)/(CHANNEL_MAX-CHANNEL_MIN);
00064 }
00065 float chToPWM(uint16_t channel){
00066     return normToPWM(normChannel(channel));
00067 }
00068 
00069 void channelsToEsc() {
00070     if (!isInitializated()) {
00071         pc.printf("WARNING!!!! NOT INITIALIZATED!!!!\r\n");
00072         return;    
00073     }
00074     depthEsc.pulsewidth_us(chToPWM(channels[CHANNEL_DEPTH])); // servo position determined by a pulsewidth between 1-2ms
00075     
00076     float forwardFactor = (1.0f-normChannel(channels[CHANNEL_THROTTLE]))-0.5f;
00077     float yawFactor     = normChannel(channels[CHANNEL_YAW])-0.5f;
00078     pc.printf("\t\t\t\tff: %f; yf: %f\r\n", forwardFactor, yawFactor);
00079     leftEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500+yawFactor*500);
00080     rightEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500-yawFactor*500);
00081 }
00082 
00083 int main() {
00084     
00085     depthEsc.period_us(20000);          // servo requires a 20ms period
00086     depthEsc.pulsewidth_us(SERVO_CENTER);
00087     leftEsc.period_us(20000);          // servo requires a 20ms period
00088     leftEsc.pulsewidth_us(SERVO_CENTER);
00089     rightEsc.period_us(20000);          // servo requires a 20ms period
00090     rightEsc.pulsewidth_us(SERVO_CENTER);
00091     
00092     
00093     //v_servo.pulsewidth_us(pulse); // servo position determined by a pulsewidth between 1-2ms
00094     
00095     //radio.powerUp();
00096     pc.baud(115200);
00097     pc.printf("We all live in a yellow submarine!\r\n");
00098     
00099     radio.setRxAddress(0xDEADBEEF0F);
00100     radio.setTxAddress(0xDEADC0DE0F);
00101     radio.setRfFrequency(2405);
00102     radio.powerUp();
00103  
00104     pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  radio.getRfFrequency() );
00105     pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  radio.getRfOutputPower() );
00106     pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", radio.getAirDataRate() );
00107     pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", radio.getTxAddress() );
00108     pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", radio.getRxAddress() );
00109     
00110     /*while (radio.getRxAddress()!=0xDEADBEEF0F){
00111         pc.printf("RX address fail!\r\n");
00112         radio.powerDown();
00113         radio.setRxAddress(0xDEADBEEF0F);
00114         radio.powerUp();
00115     }*/
00116  
00117     radio.setTransferSize( CHANNELS_NUMBER*sizeof(uint16_t) );
00118  
00119     radio.setReceiveMode();
00120  
00121     radio.enable();
00122     
00123     pc.printf("Greetings, capitain!\r\n");
00124     pc.printf("is initializated: %s\r\n", isInitializated()?"true":"false");
00125     pc.printf( "0x%X\r\n", state );
00126     t.start();
00127     
00128     while (1)
00129     {
00130         int startTime=t.read_ms();
00131         while( !radio.readable() && t.read_ms()-startTime < RCV_TIMEOUT_MS);
00132         
00133         if(!radio.readable()) {
00134             //failsafe
00135             setState(STATE_FAILSAFE);
00136             pc.printf("Failsafe\n\r", channels[0],channels[1],channels[2],channels[3]);
00137             channels[0]=512;
00138             channels[1]=512;
00139             channels[2]=512;
00140             channels[3]=512;
00141         } else {
00142             int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
00143             
00144             if (buffer[0] == CMD_VALUE) {
00145                 pc.printf("Command: %d\r\n", buffer[1]);
00146                 switch(buffer[1]){
00147                     case CMD_GET_STATUS:
00148                         zeroBuffer();
00149                         //strcpy((char*)buffer,"I'm alive");
00150                         buffer[0]=state;
00151                         buffer[1]=t.read_us();
00152                         radio.setTransmitMode();
00153                         //wait(0.1);
00154                         bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
00155                         if (bytesWritten<2*CHANNELS_NUMBER){
00156                             pc.printf("Transmit error\r\n");
00157                         } else {
00158                             //pc.printf("Sent telemetry: %s\r\n",(char*)buffer);
00159                         }
00160                         radio.setReceiveMode();
00161                         //wait(0.1);
00162                     break;
00163                     case CMD_INITIALIZE:
00164                         initialize();
00165                         setState(STATE_RUN);
00166                     break;
00167                 }    
00168             } else {
00169                 for(int i=0; i<CHANNELS_NUMBER; i++){
00170                     channels[i] = buffer[i];
00171                 }
00172                 channelsToEsc();
00173                 pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]);
00174             }
00175         }
00176         
00177         
00178     }
00179     
00180 }