四轴飞行器程序下载

#include "mQuad.h"
#include <Microduino_Protocol.h>
#include <stdlib.h>
#include <EEPROM.h>
#define EEPROM_ADDR 0
byte eeprom_data = 0;

#include <RF24Network.h>
RF24 radio(D9, D10);
RF24Network network(radio);

const uint16_t this_node = 00;
const uint16_t other_node = 01;


#define CHANNEL_NUM 8


struct receive_a {
  uint32_t ms;
  uint16_t rf_CH[CHANNEL_NUM];
};
struct send_a { //鍙戦��
  uint32_t node_ms;   //鑺傜偣杩愯鏃堕棿
};


//ProtocolSer protocol(&Serial, 16);  //閲囩敤ProSerial锛屾暟鎹暱搴︿负16涓瓧鑺�

uint8_t recCmd;
uint16_t Data[8];
uint16_t dataBuf[4] = {1000, 1000, 1000, 1000};
uint32_t spiTimer;
uint8_t spiCmd;
uint8_t spiCmdData;
uint8_t spiFlag = 0;
bool ITflag = 0;


//新添加的变量
enum state_t {
  state_init = 0, state_check_matched,  state_public_channel_init,  state_public_channel,  state_priviate_channel_init, state_priviate_channel, state_rotate
};

#define PRIVIATE_CHN_MIN   10         //随机生成通道范围下限
#define PRIVIATE_CHN_MAX   50       //随机生成通道范围上限
#define LOST_TIM_MAX      1500
#define PUBLIC_CHANNEL     0

struct payload_public_t {                  // 数据的结构体
  int priviate_chn;
};

int _state = state_init;
int priviate_channel = 0;
int rotate_flag = 0;
int unlock_cnt = 0; 
unsigned long now_1, now_2, now_3, now_4, tim_1, tim_2 = 0;
;
void setup() {
  // initialize both serial ports:
  digitalWrite(QUAD_CS, HIGH);
  QuadBegin();
  delay(100);
  Serial.begin(9600);  //9600/19200/38400
  Serial.println("RF24Network/examples/helloworld_rx/");
  spiTimer = millis();
  _state = state_init;
}

void loop()
{
 /*
  if(!(radio.isPVariant()))
{
    // initialize both serial ports:
  QuadBegin();
  delay(100);
  Serial.begin(9600);  //9600/19200/38400
  //spiTimer = millis();
  //_state = state_init;   
  }
  */
  //Get_SoftDeviceVersion();
  //先从遥控器接收
  pinMode(QUAD_CS, OUTPUT);
  digitalWrite(QUAD_CS, HIGH);

  switch (_state) {
    case state_init: {
        _state = state_check_matched;
        break;
      }
      
    case state_check_matched: {
        eeprom_data = EEPROM.read(EEPROM_ADDR);
        if (eeprom_data == 0)
        {
          _state = state_public_channel_init;
          rotate_flag = 0;
        }
        else
          _state = state_rotate;
        break;
      }

    case state_public_channel_init: {
        radio.begin();
        radio.setDataRate(RF24_250KBPS);
        network.begin(/*channel*/ PUBLIC_CHANNEL, /*node address*/ this_node);
        Serial.println("state_public_channel_init success");
        now_1 = millis();
        _state = state_public_channel;
        break;
      }

    //公共通道配对，并接收priviate_chn信息
    case state_public_channel: {
        network.update();
        while (network.available()) {
          now_1 = millis();
          RF24NetworkHeader header;
          payload_public_t payload_public;
          network.read(header, &payload_public, sizeof(payload_public)); //读取数据
          priviate_channel = payload_public.priviate_chn;
          EEPROM.write(EEPROM_ADDR, priviate_channel); //收到了新的 就写进去
          Serial.println("state_public_channel match success");
          Serial.print("The new channel is ");
          Serial.println(priviate_channel);
          _state = state_priviate_channel_init;
        }
        now_2 = millis();
        if (now_2 - now_1 > LOST_TIM_MAX && rotate_flag == 1)
          _state = state_priviate_channel_init;
        else if (now_2 - now_1 > LOST_TIM_MAX)
          _state = state_public_channel_init;
        break;
      }

    //设置新的通道
    case state_priviate_channel_init: {
        priviate_channel = EEPROM.read(EEPROM_ADDR);
        radio.begin();
        radio.setDataRate(RF24_250KBPS);
        network.begin(/*channel*/ priviate_channel, /*node address*/ this_node);
        Serial.println("state_priviate_channel_init success");
        Serial.print("The priviate_channel is : ");
        Serial.print(priviate_channel);
        _state = state_priviate_channel;
        now_3 = millis();
        break;
      }

    //新通道配对，并接收新通道的信息
    case state_priviate_channel: {
        network.update();
        if (network.available())
        {
          now_3 = millis();
          RF24NetworkHeader header;
          receive_a rec;
          network.read(header, &rec, sizeof(rec));
          for (int a = 0; a < CHANNEL_NUM; a++) {
            Data[a] = rec.rf_CH[a];
          }
          Data[0] = Data[0] ;
          Data[1] = Data[1] ;

          //protocol.readWords(&recCmd, Data, 8);    
          spiFlag = 1;                               //从joypad收到了数据
          Data[0] = map(Data[0], 1000, 2000, 2000, 1000);
          Data[1] = map(Data[1], 1000, 2000, 2000, 1000);
          RF24NetworkHeader talker(other_node);
            
          network.write(talker,  Data_to_Rc, sizeof(Data_to_Rc));
        } 
          //后向四轴发送
          if (millis() - spiTimer > 10)
          {
            pinMode(QUAD_CS, OUTPUT);
            digitalWrite(QUAD_CS, LOW);
            pinMode(NRF_CS, OUTPUT);
            digitalWrite(NRF_CS, HIGH);
            spiTimer = millis();
            tim_1 = millis();
            if (spiFlag == 0) {
              spiCmd = CMD_NONE;
            }
            else if (Data[4] != dataBuf[0]) {
              dataBuf[0] = Data[4];
              spiCmd = CMD_AUTO_FLY_UP;
            }
            else if (Data[5] != dataBuf[1]) {
              dataBuf[1] = Data[5];
              spiCmd = CMD_AUTO_LANDING;
            }
            else if (Data[6] != dataBuf[2]) {
              dataBuf[2] = Data[6];
              spiCmd = CMD_ACC_CLI;
            }
            //解锁
            
            else if (Data[0] < 1300 && Data[1] > 1600 && Data[2] < 1300 && Data[3] < 1300 && Data[7] == 2000 && unlock_cnt++ > 25 && (tim_1 - tim_2) > 4500 ) {
              tim_2 = millis();              
              spiCmd = CMD_ARM_IT_STICK;
              ITflag = 1;
              spiCmdData = ITflag;
              unlock_cnt = 0;
            }
             //上锁
            else if (Data[0] > 1600 && Data[1] > 1600 && Data[2] > 1600 && Data[3] < 1300 ) {
              spiCmd = CMD_ARM_IT_STICK;
              ITflag = 0;
              spiCmdData = ITflag;
            }
            
            else if ( Data[7] == 1000 ) {
              spiCmd = CMD_ARM_IT_SWITCH;
              ITflag = 0;
              spiCmdData = ITflag;
            }

            else {
              spiCmd = CMD_SET_CH4;
            }

            spiFlag = 0;
            QuadWrite(spiCmd, spiCmdData, Data);
                                        
          }
        
        now_4 = millis();
        if (now_4 - now_3 > LOST_TIM_MAX)
          _state = state_rotate;
        break;
      }

    case state_rotate: {
        rotate_flag = 1;
        _state = state_public_channel_init;
        
      }
    default:
      return 0;

  }
}