“自平衡小车/zh”的版本间的差异

来自Microduino Wikipedia
跳转至： 导航、 搜索
（→‎程序说明）
第194行： 第194行：
 
*调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池
 
*调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池
 
==程序说明==
 
==程序说明==
 +
 +
==Joypad程序及说明==
 +
Joypad_RC.ino
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
#include "def.h"
 +
#include "time.h"
 +
#include "bat.h"
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
#include "mpu.h"
 +
#endif
 +
#include "joy.h"
 +
#include "key.h"
 +
#include "data.h"
 +
#include "nrf.h"
 +
#include "mwc.h"
 +
#include "tft.h"
 +
#include "eep.h"
 +
 +
#if defined(__AVR_ATmega128RFA1__)
 +
#include <ZigduinoRadio.h>
 +
#endif
 +
 +
//joypad================================
 +
#include <Joypad.h>
 +
//eeprom================================
 +
#include <EEPROM.h>
 +
//TFT===================================
 +
#include <Adafruit_GFX.h>    // Core graphics library
 +
#include <Adafruit_ST7735.h> // Hardware-specific
 +
#include <SPI.h>
 +
//rf====================================
 +
#include <RF24Network.h>
 +
#include <RF24.h>
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
//MPU===================================
 +
#include "Wire.h"
 +
#include "I2Cdev.h"
 +
#include "MPU6050_6Axis_MotionApps20.h"
 +
#endif
 +
 +
//spi===================================
 +
#include <SPI.h>
 +
 +
void setup()
 +
{
 +
  // initialize serial communication at 115200 bits per second:
 +
 +
#ifdef Serial_DEBUG
 +
  Serial.begin(115200);
 +
  delay(100);
 +
  Serial.println("========hello========");
 +
#endif
 +
 +
  //---------------
 +
  key_init();
 +
 +
  //---------------
 +
#ifdef Serial_DEBUG
 +
  Serial.println("\n\r EEPROM READ...");
 +
#endif
 +
  eeprom_read();
 +
 +
  //---------------
 +
#ifdef Serial_DEBUG
 +
  Serial.println("\n\r TFT INIT...");
 +
#endif
 +
  TFT_init(true, tft_rotation);
 +
 +
  //---------------
 +
#ifdef Serial_DEBUG
 +
  Serial.println("\n\r TFT BEGIN...");
 +
#endif
 +
  TIME1 = millis();
 +
  while (millis() - TIME1 < interval_TIME1)
 +
  {
 +
    TFT_begin();
 +
 +
    if (!Joypad.readButton(CH_SWITCH_1))
 +
    {
 +
#ifdef Serial_DEBUG
 +
      Serial.println("\n\rCorrect IN...");
 +
#endif
 +
 +
      //---------------
 +
#ifdef Serial_DEBUG
 +
      Serial.println("\n\r TFT INIT...");
 +
#endif
 +
      TFT_init(false, tft_rotation);
 +
 +
      while (1)
 +
      {
 +
        if (!TFT_config())
 +
          break;
 +
      }
 +
#ifdef Serial_DEBUG
 +
      Serial.println("\n\rCorrect OUT...");
 +
#endif
 +
 +
      //---------------
 +
#ifdef Serial_DEBUG
 +
      Serial.println("\n\r EEPROM WRITE...");
 +
#endif
 +
      eeprom_write();
 +
    }
 +
  }
 +
 +
  //---------------
 +
#ifdef Serial_DEBUG
 +
  Serial.println("\n\r TFT CLEAR...");
 +
#endif
 +
  TFT_clear();
 +
 +
  //---------------
 +
#ifdef Serial_DEBUG
 +
  Serial.println("\n\r TFT READY...");
 +
#endif
 +
  TFT_ready();
 +
 +
  //---------------.l
 +
  if (mode_protocol)  //Robot
 +
  {
 +
    SPI.begin(); //初始化SPI总线
 +
    radio.begin();
 +
    network.begin(/*channel*/ nrf_channal, /*node address*/ this_node);
 +
  }
 +
  else          //QuadCopter
 +
  {
 +
    unsigned long _channel;
 +
#if !defined(__AVR_ATmega128RFA1__)
 +
    switch (mwc_channal)
 +
    {
 +
      case 0:
 +
        _channel = 9600;
 +
        break;
 +
      case 1:
 +
        _channel = 19200;
 +
        break;
 +
      case 2:
 +
        _channel = 38400;
 +
        break;
 +
      case 3:
 +
        _channel = 57600;
 +
        break;
 +
      case 4:
 +
        _channel = 115200;
 +
        break;
 +
    }
 +
#else if
 +
    _channel = mwc_channal;
 +
#endif
 +
    mwc_port.begin(_channel);
 +
  }
 +
 +
  //---------------
 +
#ifdef Serial_DEBUG
 +
  Serial.println("===========start===========");
 +
#endif
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
  if (mode_mpu) initMPU(); //initialize device
 +
#endif
 +
}
 +
 +
void loop()
 +
{
 +
  //  unsigned long time = millis();
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
  //MPU--------------------------------
 +
  if (mode_mpu)
 +
    getMPU();
 +
#endif
 +
 +
  //DATA_begin------------------------------
 +
  data_begin();
 +
 +
  //DATA_send-------------------------------
 +
  if (millis() < time2) time2 = millis();
 +
  if (millis() - time2 > interval_time2)
 +
  {
 +
    if (mode_protocol) nrf_send();    //Robot
 +
    else data_send();          //QuadCopter
 +
 +
    time2 = millis();
 +
  }
 +
 +
  //节点查错-------------------------------
 +
  vodebug();
 +
 +
  //BAT--------------------------------
 +
  if (time3 > millis()) time3 = millis();
 +
  if (millis() - time3 > interval_time3)
 +
  {
 +
    vobat();
 +
    time3 = millis();
 +
  }
 +
 +
  //TFT------------------------------------
 +
  TFT_run();
 +
 +
  //===================================
 +
  //  time = millis() - time;
 +
 +
  //  Serial.println(time, DEC);    //loop time
 +
}
 +
</cpp>
 +
BAT.h
 +
<source lang="cpp">
 +
int8_t _V_bat = _V_min;
 +
 +
boolean mcu_voltage = true; // 5.0 or 3.3
 +
#define _V_fix 0.2  //fix battery voltage
 +
#define _V_math(Y) (_V_fix+((Y*analogRead(PIN_bat)/1023.0f)/(33.0f/(51.0f+33.0f))))
 +
 +
void vobat()
 +
{
 +
  //_V_bat=10*((voltage*analogRead(PIN_bat)/1023.0f)/(33.0f/(51.0f+33.0f)));
 +
  _V_bat = _V_math(mcu_voltage ? 50 : 33);
 +
  _V_bat = constrain(_V_bat, _V_min, _V_max);
 +
 +
#ifdef Serial_DEBUG
 +
  Serial.print("_V_bat: ");
 +
  Serial.println(_V_bat);
 +
#endif
 +
}
 +
</source>
 +
data.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
byte inBuf[16];
 +
 +
int16_t outBuf[8] =
 +
{
 +
  Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID
 +
};
 +
 +
boolean AUX[4] = {0, 0, 0, 0};
 +
//======================================
 +
void data_begin()
 +
{
 +
  Joy();
 +
 +
  if (mode_protocol)  //Robot
 +
  {
 +
    if (!sw_l)
 +
    {
 +
      Joy_x = Joy_MID;
 +
      Joy_y = Joy_MID;
 +
      Joy1_x = Joy_MID;
 +
      Joy1_y = Joy_MID;
 +
    }
 +
  }
 +
  else        //QuadCopter
 +
  {
 +
    if (!sw_l)
 +
      Joy_y = Joy_MID - Joy_maximum;
 +
  }
 +
 +
  //but---------------------------------
 +
  for (uint8_t a = 0; a < 4; a++)
 +
  {
 +
    if (key_get(a, 1))  AUX[a] = !AUX[a];
 +
  }
 +
 +
  outBuf[0] = Joy1_x;
 +
  outBuf[1] = Joy1_y;
 +
  outBuf[2] = Joy_x;
 +
  outBuf[3] = Joy_y;
 +
  outBuf[4] = map(AUX[0], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
  outBuf[5] = map(AUX[1], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
  outBuf[6] = map(AUX[2], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
  outBuf[7] = map(AUX[3], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
}
 +
 +
</source>
 +
def.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
//DEBUG-----------
 +
#define Serial_DEBUG
 +
 +
//MWC-------------
 +
uint8_t mwc_channal = 11; //RF channel
 +
 +
#if  defined(__AVR_ATmega32U4__)
 +
#define mwc_port Serial1    //Serial1 is D0 D1
 +
#elif defined(__AVR_ATmega128RFA1__)
 +
#define mwc_port ZigduinoRadio    //RF
 +
#else
 +
#define mwc_port Serial    //Serial is D0 D1
 +
#endif
 +
 +
//nRF-------------
 +
#define interval_debug  2000  //节点查错间隔
 +
uint8_t nrf_channal = 70;  //0~125
 +
 +
//Battery---------
 +
#define PIN_bat A7 //BAT
 +
 +
#define _V_max 41 //锂电池满电电压4.2V
 +
#define _V_min 36 //锂电池没电电压3.7V
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
//MPU-------------
 +
#define MPU_maximum 70
 +
#endif
 +
 +
 +
//Time------------
 +
#define interval_TIME1 2000    //setup delay
 +
#define interval_time2 40      //send interval
 +
#define interval_time3 1000    //battery interval
 +
</source>
 +
eep.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
#include <EEPROM.h>
 +
 +
#define EEPROM_write(address, p) {int i = 0; byte *pp = (byte*)&(p);for(; i < sizeof(p); i++) EEPROM.write(address+i, pp[i]);}
 +
#define EEPROM_read(address, p)  {int i = 0; byte *pp = (byte*)&(p);for(; i < sizeof(p); i++) pp[i]=EEPROM.read(address+i);}
 +
 +
struct config_type
 +
{
 +
  int16_t eeprom_correct_min[4];
 +
  int16_t eeprom_correct_max[4];
 +
  uint8_t eeprom_Joy_deadzone_val;
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
  boolean eeprom_mode_mpu;
 +
#endif
 +
  boolean eeprom_mode_protocol;
 +
  uint8_t eeprom_mwc_channal;
 +
  uint8_t eeprom_nrf_channal;
 +
  boolean eeprom_tft_theme;
 +
  boolean eeprom_tft_rotation;
 +
  boolean eeprom_mcu_voltage;
 +
};
 +
 +
//======================================
 +
void eeprom_read()
 +
{
 +
  //EEPROM读取赋值
 +
  config_type config_readback;
 +
  EEPROM_read(0, config_readback);
 +
 +
  for (uint8_t a = 0; a < 4; a++)
 +
  {
 +
    joy_correct_min[a] = config_readback.eeprom_correct_min[a];
 +
    joy_correct_max[a] = config_readback.eeprom_correct_max[a];
 +
  }
 +
  Joy_deadzone_val = config_readback.eeprom_Joy_deadzone_val;
 +
 +
  mode_protocol = config_readback.eeprom_mode_protocol;
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
  mode_mpu = config_readback.eeprom_mode_mpu;
 +
#endif
 +
 +
  mwc_channal = config_readback.eeprom_mwc_channal;
 +
  nrf_channal = config_readback.eeprom_nrf_channal;
 +
  tft_theme = config_readback.eeprom_tft_theme;
 +
  tft_rotation = config_readback.eeprom_tft_rotation;
 +
  mcu_voltage = config_readback.eeprom_mcu_voltage;
 +
}
 +
 +
void eeprom_write()
 +
{
 +
  // 定义结构变量config，并定义config的内容
 +
  config_type config;
 +
 +
  for (uint8_t a = 0; a < 4; a++)
 +
  {
 +
    config.eeprom_correct_min[a] = joy_correct_min[a];
 +
    config.eeprom_correct_max[a] = joy_correct_max[a];
 +
  }
 +
  config.eeprom_Joy_deadzone_val = Joy_deadzone_val;
 +
 +
  config.eeprom_mode_protocol = mode_protocol;
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
  config.eeprom_mode_mpu = mode_mpu;
 +
#endif
 +
 +
  config.eeprom_mwc_channal = mwc_channal;
 +
  config.eeprom_nrf_channal = nrf_channal;
 +
  config.eeprom_tft_theme = tft_theme;
 +
  config.eeprom_tft_rotation = tft_rotation;
 +
  config.eeprom_mcu_voltage = mcu_voltage;
 +
 +
  // 变量config存储到EEPROM，地址0写入
 +
  EEPROM_write(0, config);
 +
}
 +
</source>
 +
joy.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
#include <Joypad.h>
 +
 +
//Joy-------------
 +
//1000~2000
 +
uint8_t Joy_deadzone_val = 10;
 +
#define Joy_s_maximum 200 //MAX 300
 +
#define Joy_maximum 450 //MAX 500
 +
#define Joy_MID 1500  //1500
 +
 +
boolean mode_mpu, mode_protocol;  //{(0: 0 is mwc, 1 is nrf),(1: 0 is mpu, 1 is no mpu)}
 +
 +
int16_t joy_correct_max[4], joy_correct_min[4];
 +
int16_t Joy_x, Joy_y, Joy1_x, Joy1_y;
 +
 +
int16_t s_lig, s_mic;
 +
 +
boolean Joy_sw, Joy1_sw;
 +
 +
boolean but1, but2, but3, but4;
 +
 +
boolean sw_l, sw_r;
 +
 +
//======================================
 +
int16_t Joy_dead_zone(int16_t _Joy_vol)
 +
{
 +
  if (abs(_Joy_vol) > Joy_deadzone_val)
 +
    return ((_Joy_vol > 0) ? (_Joy_vol - Joy_deadzone_val) : (_Joy_vol + Joy_deadzone_val));
 +
  else
 +
    return 0;
 +
}
 +
 +
int16_t Joy_i(int16_t _Joy_i, boolean _Joy_b, int16_t _Joy_MIN, int16_t _Joy_MAX)
 +
{
 +
  int16_t _Joy_a;
 +
  switch (_Joy_i)
 +
  {
 +
    case 0:
 +
      _Joy_a = Joy_dead_zone(Joypad.readJoystickX());
 +
      break;
 +
    case 1:
 +
      _Joy_a = Joypad.readJoystickY();    //throt
 +
      break;
 +
    case 2:
 +
      _Joy_a = Joy_dead_zone(Joypad.readJoystick1X());
 +
      break;
 +
    case 3:
 +
      _Joy_a = Joy_dead_zone(Joypad.readJoystick1Y());
 +
      break;
 +
  }
 +
 +
  if (_Joy_b)
 +
  {
 +
    if (_Joy_a < 0)
 +
      _Joy_a = map(_Joy_a, joy_correct_min[_Joy_i], 0, _Joy_MAX, Joy_MID);
 +
    else
 +
      _Joy_a = map(_Joy_a, 0, joy_correct_max[_Joy_i], Joy_MID, _Joy_MIN);
 +
 +
    if (_Joy_a < _Joy_MIN) _Joy_a = _Joy_MIN;
 +
    if (_Joy_a > _Joy_MAX) _Joy_a = _Joy_MAX;
 +
  }
 +
  return _Joy_a;
 +
}
 +
 +
void Joy()
 +
{
 +
  sw_l = Joypad.readButton(CH_SWITCH_L);
 +
  sw_r = Joypad.readButton(CH_SWITCH_R);
 +
 +
  //------------------------------------
 +
  //s_lig=Joypad.readLightSensor();
 +
  //s_mic=Joypad.readMicrophone();
 +
 +
  //------------------------------------
 +
  Joy_sw = Joypad.readButton(CH_JOYSTICK_SW);
 +
  Joy1_sw = Joypad.readButton(CH_JOYSTICK1_SW);
 +
 +
  //------------------------------------
 +
  but1 = Joypad.readButton(CH_SWITCH_1);
 +
  but2 = Joypad.readButton(CH_SWITCH_2);
 +
  but3 = Joypad.readButton(CH_SWITCH_3);
 +
  but4 = Joypad.readButton(CH_SWITCH_4);
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
  //====================================
 +
  int16_t y[3];        //MPU---------------------------------
 +
  if (mode_mpu)    //MPU---------------------------------
 +
  {
 +
    for (uint8_t a = 0; a < 3; a++)
 +
    {
 +
      y[a] = ypr[a] * 180 / M_PI;
 +
      if (y[a] > MPU_maximum) y[a] = MPU_maximum;
 +
      if (y[a] < -MPU_maximum) y[a] = -MPU_maximum;
 +
    }
 +
  }
 +
#endif
 +
 +
  if (Joypad.readButton(CH_SWITCH_R))
 +
  {
 +
    Joy_x = Joy_i(0, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
    Joy_y = Joy_i(1, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
    if (mode_mpu)    //MPU---------------------------------
 +
    {
 +
      Joy1_x = map(y[2], -MPU_maximum, MPU_maximum, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
      Joy1_y = map(y[1], -MPU_maximum, MPU_maximum, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
    }
 +
    else
 +
#endif
 +
    {
 +
      Joy1_x = Joy_i(2, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
      Joy1_y = Joy_i(3, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
    }
 +
  }
 +
  else
 +
  {
 +
    Joy_x = Joy_i(0, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
 +
    Joy_y = Joy_i(1, true, mode_protocol ? Joy_MID - Joy_s_maximum : Joy_MID - Joy_maximum, mode_protocol ? Joy_MID + Joy_s_maximum : Joy_MID + Joy_maximum); //  Robot,QuadCopter
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
    if (mode_mpu)    //MPU---------------------------------
 +
    {
 +
      Joy1_x = map(y[2], -MPU_maximum, MPU_maximum, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
 +
      Joy1_y = map(y[1], -MPU_maximum, MPU_maximum, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
 +
    }
 +
    else
 +
#endif
 +
    {
 +
      Joy1_x = Joy_i(2, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
 +
      Joy1_y = Joy_i(3, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
 +
    }
 +
  }
 +
}
 +
 +
</source>
 +
key.h
 +
<source lang="cpp">
 +
#include "arduino.h"
 +
 +
uint8_t key_pin[4] = {CH_SWITCH_1, CH_SWITCH_2, CH_SWITCH_3, CH_SWITCH_4}; //按键1 2 3 4
 +
 +
boolean key_status[4]; //按键
 +
boolean key_cache[4]; //检测按键松开缓存
 +
 +
void key_init()
 +
{
 +
  for (uint8_t a = 0; a < 4; a++)
 +
  {
 +
    key_status[a] = LOW;
 +
    key_cache[a] = HIGH;
 +
  }
 +
}
 +
 +
boolean key_get(uint8_t _key_num, boolean _key_type)
 +
{
 +
  key_cache[_key_num] = key_status[_key_num]; //缓存作判断用
 +
 +
  key_status[_key_num] = !Joypad.readButton(key_pin[_key_num]); //触发时
 +
 +
  switch (_key_type)
 +
  {
 +
    case 0:
 +
      if (!key_status[_key_num] && key_cache[_key_num]) //按下松开后
 +
        return true;
 +
      else
 +
        return false;
 +
      break;
 +
    case 1:
 +
      if (key_status[_key_num] && !key_cache[_key_num]) //按下松开后
 +
        return true;
 +
      else
 +
        return false;
 +
      break;
 +
  }
 +
}
 +
</source>
 +
mpu.h
 +
<source lang="cpp">
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
#include "Wire.h"
 +
#include "I2Cdev.h"
 +
#include "MPU6050_6Axis_MotionApps20.h"
 +
 +
MPU6050 mpu;
 +
 +
//MPU-------------
 +
#define MPU_maximum 70
 +
 +
// MPU control/status vars
 +
boolean dmpReady = false;  // set true if DMP init was successful
 +
uint8_t mpuIntStatus;  // holds actual interrupt status byte from MPU
 +
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
 +
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
 +
uint16_t fifoCount;    // count of all bytes currently in FIFO
 +
uint8_t fifoBuffer[64]; // FIFO storage buffer
 +
 +
// orientation/motion vars
 +
Quaternion q;          // [w, x, y, z]        quaternion container
 +
VectorInt16 aa;        // [x, y, z]            accel sensor measurements
 +
VectorFloat gravity;    // [x, y, z]            gravity vector
 +
float ypr[3];          // [yaw, pitch, roll]  yaw/pitch/roll container and gravity vector
 +
 +
void initMPU()
 +
{
 +
  Wire.begin();
 +
#ifdef Serial_DEBUG
 +
  Serial.println(F("Initializing I2C devices..."));
 +
#endif
 +
  mpu.initialize();
 +
  // verify connection
 +
#ifdef Serial_DEBUG
 +
  Serial.println(F("Testing device connections..."));
 +
#endif
 +
  if (mpu.testConnection())
 +
  {
 +
#ifdef Serial_DEBUG
 +
    Serial.println("MPU6050 connection successful");
 +
#endif
 +
  }
 +
#ifdef Serial_DEBUG
 +
  else
 +
    Serial.println(F("MPU6050 connection failed"));
 +
#endif
 +
 +
  // load and configure the DMP
 +
#ifdef Serial_DEBUG
 +
  Serial.println(F("Initializing DMP..."));
 +
#endif
 +
  devStatus = mpu.dmpInitialize();
 +
 +
  // make sure it worked (returns 0 if so)
 +
  if (devStatus == 0) {
 +
    // turn on the DMP, now that it's ready
 +
#ifdef Serial_DEBUG
 +
    Serial.println(F("Enabling DMP..."));
 +
#endif
 +
    mpu.setDMPEnabled(true);
 +
 +
    mpuIntStatus = mpu.getIntStatus();
 +
 +
    // set our DMP Ready flag so the main loop() function knows it's okay to use it
 +
    //    Serial.println(F("DMP ready! Waiting for first interrupt..."));
 +
    dmpReady = true;
 +
 +
    // get expected DMP packet size for later comparison
 +
    packetSize = mpu.dmpGetFIFOPacketSize();
 +
  }
 +
  else {
 +
    // ERROR!
 +
    // 1 = initial memory load failed
 +
    // 2 = DMP configuration updates failed
 +
    // (if it's going to break, usually the code will be 1)
 +
#ifdef Serial_DEBUG
 +
    Serial.print(F("DMP Initialization failed (code "));
 +
    Serial.print(devStatus);
 +
    Serial.println(F(")"));
 +
#endif
 +
  }
 +
}
 +
 +
void getMPU()
 +
{
 +
  if (!dmpReady) return;
 +
  {
 +
    // reset interrupt flag and get INT_STATUS byte
 +
    mpuIntStatus = mpu.getIntStatus();
 +
 +
    // get current FIFO count
 +
    fifoCount = mpu.getFIFOCount();
 +
 +
    // check for overflow (this should never happen unless our code is too inefficient)
 +
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
 +
    {
 +
      // reset so we can continue cleanly
 +
      mpu.resetFIFO();
 +
#ifdef Serial_DEBUG
 +
      Serial.println(F("FIFO overflow!"));
 +
#endif
 +
      // otherwise, check for DMP data ready interrupt (this should happen frequently)
 +
    }
 +
    else if (mpuIntStatus & 0x02)
 +
    {
 +
      // wait for correct available data length, should be a VERY short wait
 +
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 +
 +
      // read a packet from FIFO
 +
      mpu.getFIFOBytes(fifoBuffer, packetSize);
 +
 +
      // track FIFO count here in case there is > 1 packet available
 +
      // (this lets us immediately read more without waiting for an interrupt)
 +
      fifoCount -= packetSize;
 +
 +
      // display ypr angles in degrees
 +
      mpu.dmpGetQuaternion(&q, fifoBuffer);
 +
      mpu.dmpGetGravity(&gravity, &q);
 +
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 +
 +
      //Serial.print("ypr\t");
 +
      //Serial.print(ypr[0] * 180/M_PI);
 +
      //Serial.print("\t");
 +
      //Serial.print(ypr[1] * 180/M_PI);
 +
      //Serial.print("\t");
 +
      // Serial.println(ypr[2] * 180/M_PI);
 +
    }
 +
  }
 +
}
 +
 +
#endif
 +
</source>
 +
mwc.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
#if defined(__AVR_ATmega128RFA1__)
 +
#include <ZigduinoRadio.h>
 +
#endif
 +
 +
int16_t RCin[8], RCoutA[8], RCoutB[8];
 +
 +
int16_t p;
 +
uint16_t read16()
 +
{
 +
  uint16_t r = (inBuf[p++] & 0xFF);
 +
  r += (inBuf[p++] & 0xFF) << 8;
 +
  return r;
 +
}
 +
 +
uint16_t t, t1, t2;
 +
uint16_t write16(boolean a)
 +
{
 +
  if (a)
 +
  {
 +
    t1 = outBuf[p++] >> 8;
 +
    t2 = outBuf[p - 1] - (t1 << 8);
 +
    t = t1;
 +
  }
 +
  else
 +
    t = t2;
 +
  return t;
 +
}
 +
 +
typedef  unsigned char byte;
 +
byte getChecksum(byte length, byte cmd, byte mydata[])
 +
{
 +
  //三个参数分别为： 数据长度  ，  指令代码  ，  实际数据数组
 +
  byte checksum = 0;
 +
  checksum ^= (length & 0xFF);
 +
  checksum ^= (cmd & 0xFF);
 +
  for (uint8_t i = 0; i < length; i++)
 +
  {
 +
    checksum ^= (mydata[i] & 0xFF);
 +
  }
 +
  return checksum;
 +
}
 +
 +
void data_rx()
 +
{
 +
  //  s_struct_w((int*)&inBuf,16);
 +
  p = 0;
 +
  for (uint8_t i = 0; i < 8; i++)
 +
  {
 +
    RCin[i] = read16();
 +
    /*
 +
    Serial.print("RC[");
 +
    Serial.print(i+1);
 +
    Serial.print("]:");
 +
 +
    Serial.print(inBuf[2*i],DEC);
 +
    Serial.print(",");
 +
    Serial.print(inBuf[2*i+1],DEC);
 +
 +
    Serial.print("---");
 +
    Serial.println(RCin[i]);
 +
    */
 +
    //    delay(50);        // delay in between reads for stability
 +
  }
 +
}
 +
 +
void data_tx()
 +
{
 +
  p = 0;
 +
  for (uint8_t i = 0; i < 8; i++)
 +
  {
 +
    RCoutA[i] = write16(1);
 +
    RCoutB[i] = write16(0);
 +
 +
    /*
 +
    Serial.print("RC[");
 +
    Serial.print(i+1);
 +
    Serial.print("]:");
 +
 +
    Serial.print(RCout[i]);
 +
 +
    Serial.print("---");
 +
 +
    Serial.print(RCoutA[i],DEC);
 +
    Serial.print(",");
 +
    Serial.print(RCoutB[i],DEC);
 +
 +
    Serial.println("");
 +
    */
 +
    //    delay(50);        // delay in between reads for stability
 +
  }
 +
}
 +
 +
/*
 +
if Core RF
 +
[head,2byte,0xAA 0xBB] [type,1byte,0xCC] [data,16byte] [body,1byte(from getChecksum())]
 +
Example:
 +
AA BB CC 1A 01 1A 01 1A 01 2A 01 3A 01 4A 01 5A 01 6A 01 0D **
 +
*/
 +
void data_send()
 +
{
 +
  data_tx();
 +
 +
#if !defined(__AVR_ATmega128RFA1__)
 +
  static byte buf_head[3];
 +
  buf_head[0] = 0x24;
 +
  buf_head[1] = 0x4D;
 +
  buf_head[2] = 0x3C;
 +
#endif
 +
 +
#define buf_length 0x10  //16
 +
#define buf_code 0xC8    //200
 +
 +
  static byte buf_data[buf_length];
 +
  for (uint8_t a = 0; a < (buf_length / 2); a++)
 +
  {
 +
    buf_data[2 * a] = RCoutB[a];
 +
    buf_data[2 * a + 1] = RCoutA[a];
 +
  }
 +
 +
  static byte buf_body;
 +
  buf_body = getChecksum(buf_length, buf_code, buf_data);
 +
 +
  //----------------------
 +
#if defined(__AVR_ATmega128RFA1__)
 +
  mwc_port.beginTransmission();
 +
  mwc_port.write(0xaa);
 +
  mwc_port.write(0xbb);
 +
  mwc_port.write(0xcc);
 +
#else
 +
  for (uint8_t a = 0; a < 3; a++) {
 +
    mwc_port.write(buf_head[a]);
 +
  }
 +
  mwc_port.write(buf_length);
 +
  mwc_port.write(buf_code);
 +
#endif
 +
  for (uint8_t a = 0; a < buf_length; a++) {
 +
    mwc_port.write(buf_data[a]);
 +
  }
 +
  mwc_port.write(buf_body);
 +
#if defined(__AVR_ATmega128RFA1__)
 +
  mwc_port.endTransmission();
 +
#endif
 +
}
 +
</source>
 +
nrf.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
#include <RF24Network.h>
 +
#include <RF24.h>
 +
#include <SPI.h>
 +
 +
// nRF24L01(+) radio attached using Getting Started board
 +
RF24 radio(9, 10);  //ce,cs
 +
RF24Network network(radio);
 +
 +
#define this_node  0 //设置本机ID
 +
#define other_node 1
 +
 +
//--------------------------------
 +
struct send_a //发送
 +
{
 +
  uint32_t ms;
 +
  uint16_t rf_CH0;
 +
  uint16_t rf_CH1;
 +
  uint16_t rf_CH2;
 +
  uint16_t rf_CH3;
 +
  uint16_t rf_CH4;
 +
  uint16_t rf_CH5;
 +
  uint16_t rf_CH6;
 +
  uint16_t rf_CH7;
 +
};
 +
 +
struct receive_a //接收
 +
{
 +
  uint32_t node_ms;
 +
};
 +
 +
//--------------------------------
 +
unsigned long node_clock, node_clock_debug, node_clock_cache = 0; //节点运行时间、节点响应检查时间、节点时间缓存
 +
 +
//debug--------------------------
 +
boolean node_clock_error = false; //节点响应状态
 +
unsigned long time_debug = 0; //定时器
 +
 +
 +
//======================================
 +
void vodebug()
 +
{
 +
  if (millis() - time_debug > interval_debug)
 +
  {
 +
    node_clock_error = boolean(node_clock == node_clock_debug); //一定时间内，节点返回的运行时间若不变则有问题
 +
 +
    node_clock_debug = node_clock;
 +
 +
    time_debug = millis();
 +
  }
 +
}
 +
 +
 +
void nrf_send()
 +
{
 +
#ifdef Serial_DEBUG
 +
  Serial.print("Sending...");
 +
#endif
 +
 +
  send_a sen = {
 +
    millis(), outBuf[0], outBuf[1], outBuf[2], outBuf[3], outBuf[4], outBuf[5], outBuf[6], outBuf[7]
 +
  }; //把这些数据发送出去，对应前面的发送数组
 +
  RF24NetworkHeader header(other_node);
 +
  if (network.write(header, &sen, sizeof(sen)))
 +
  {
 +
#ifdef Serial_DEBUG
 +
    Serial.print("Is ok.");
 +
#endif
 +
 +
    delay(50);
 +
    network.update();
 +
    // If it's time to send a message, send it!
 +
    while ( network.available() )
 +
    {
 +
      // If so, grab it and print it out
 +
      RF24NetworkHeader header;
 +
      receive_a rec;
 +
      network.read(header, &rec, sizeof(rec));
 +
 +
      node_clock = rec.node_ms; //运行时间赋值
 +
    }
 +
  }
 +
#ifdef Serial_DEBUG
 +
  else
 +
    Serial.print("Is failed.");
 +
 +
  Serial.println("");
 +
#endif
 +
}
 +
</source>
 +
tft.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
#include <Adafruit_GFX.h>    // Core graphics library
 +
#include <Adafruit_ST7735.h> // Hardware-specific library
 +
#include <SPI.h>
 +
 +
Adafruit_ST7735 tft = Adafruit_ST7735(5, 4, -1);    //cs,dc,rst
 +
//-------字体设置，大、中、小
 +
#define setFont_M tft.setTextSize(2)
 +
#define setFont_S tft.setTextSize(0)
 +
 +
#define tft_width  128
 +
#define tft_height 160
 +
 +
boolean tft_theme = false;  //0 is white,1 is black
 +
boolean tft_rotation = 1;
 +
 +
#define TFT_TOP ST7735_BLACK
 +
#define TFT_BUT ST7735_WHITE
 +
 +
uint16_t  tft_colorA = TFT_BUT;
 +
uint16_t  tft_colorB = TFT_TOP;
 +
uint16_t  tft_colorC = 0x06FF;
 +
uint16_t  tft_colorD = 0xEABF;
 +
 +
#define tft_bat_x 24
 +
#define tft_bat_y 12
 +
#define tft_bat_x_s 2
 +
#define tft_bat_y_s 6
 +
 +
#define tft_font_s_height 8
 +
#define tft_font_m_height 16
 +
#define tft_font_l_height 24
 +
 +
#define _Q_x 33
 +
#define _Q_y 36
 +
#define _W_x 93
 +
#define _W_y 5
 +
 +
#define _Q_font_x 2
 +
#define _Q_font_y (_Q_y - 1)
 +
 +
int8_t tft_cache = 1;
 +
 +
//======================================
 +
void TFT_clear()
 +
{
 +
  tft.fillScreen(tft_colorB);
 +
}
 +
 +
void TFT_init(boolean _init, boolean _rot)
 +
{
 +
  tft_colorB = tft_theme ? TFT_TOP : TFT_BUT;
 +
  tft_colorA = tft_theme ? TFT_BUT : TFT_TOP;
 +
 +
  if (_init) {
 +
    tft.initR(INITR_BLACKTAB);  // initialize a ST7735S chip, black tab
 +
    //  Serial.println("init");
 +
    tft.fillScreen(tft_colorB);
 +
 +
    if (_rot)
 +
      tft.setRotation(2);
 +
  }
 +
 +
  tft.fillRect(0, 0, tft_width, 40, tft_colorA);
 +
  tft.setTextColor(tft_colorB);
 +
  setFont_M;
 +
  tft.setCursor(26, 6);
 +
  tft.print("Joypad");
 +
  setFont_S;
 +
  tft.setCursor(32, 24);
 +
  tft.print("Microduino");
 +
  tft.fillRect(0, 40, tft_width, 120, tft_colorB);
 +
}
 +
 +
void TFT_begin()
 +
{
 +
  setFont_S;
 +
 +
  tft.setTextColor(tft_colorA);
 +
  tft.setCursor(_Q_font_x, 44);
 +
  tft.println("[key1] enter config");
 +
 +
  setFont_M;
 +
  tft.setCursor(4, 150);
 +
  for (uint8_t a = 0; a < (millis() - TIME1) / (interval_TIME1 / 10); a++) {
 +
    tft.print("-");
 +
  }
 +
}
 +
 +
int8_t menu_num_A = 0;
 +
int8_t menu_num_B = 0;
 +
int8_t menu_sta = 0;
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
char *menu_str_a[5] = {
 +
  "Joystick Config", "Protocol Config", "System Config", "Gyroscope Config", "Exit"
 +
};
 +
#else
 +
char *menu_str_a[4] = {
 +
  "Joystick Config", "Protocol Config", "System Config", "Exit"
 +
};
 +
#endif
 +
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
char *menu_str_b[4][3] = {
 +
  {"Joystick Correct.", "Dead Zone config"},
 +
  {"Mode", "Quadrotor Channel", "nRF24 Channel"},
 +
  {"TFT Theme", "TFT Rotation", "MCU Voltage"},
 +
  {"Gyroscope OFF", "Gyroscope ON"}
 +
};
 +
#else
 +
char *menu_str_b[3][3] = {
 +
  {"Joystick Correct.", "Dead Zone config"},
 +
  {"Mode", "Quadrotor Channel", "nRF24 Channel"},
 +
  {"TFT Theme", "TFT Rotation", "MCU Voltage"},
 +
};
 +
#endif
 +
 +
void TFT_menu(int8_t _num, char *_data)
 +
{
 +
  tft.drawRect(7, 49 + 15 * _num, 114, 16, tft_colorA);
 +
  tft.setCursor(10, 54 + 15 * _num);
 +
  tft.print(_data);
 +
}
 +
 +
void TFT_menu(int8_t _num, int16_t _data)
 +
{
 +
  tft.drawRect(7, 49 + 15 * _num, 114, 16, tft_colorA);
 +
  tft.setCursor(10, 54 + 15 * _num);
 +
  tft.print(_data);
 +
}
 +
 +
void TFT_cursor(int8_t _num)
 +
{
 +
  tft.drawLine(1, 51 + 15 * _num, 4, 56 + 15 * _num, tft_colorA);
 +
  tft.drawLine(4, 57 + 15 * _num, 1, 62 + 15 * _num, tft_colorA);
 +
  tft.drawLine(1, 51 + 15 * _num, 1, 62 + 15 * _num, tft_colorA);
 +
}
 +
 +
boolean return_menu = false;
 +
 +
boolean TFT_config()
 +
{
 +
  tft.setTextColor( tft_colorA);
 +
 +
  if (key_get(0, 1)) {
 +
    menu_sta --;
 +
    tft_cache = 1;
 +
 +
    if (menu_sta <= 0)
 +
      menu_num_B = 0; //zero
 +
  }
 +
  if (key_get(1, 1)) {
 +
    if (return_menu)
 +
      menu_sta --;
 +
    else
 +
      menu_sta ++;
 +
    tft_cache = 1;
 +
  }
 +
 +
  if (menu_sta > 2)
 +
    menu_sta = 2;
 +
  if (menu_sta < 0)
 +
    menu_sta = 0;
 +
 +
  return_menu = false;
 +
  //-------------------------------
 +
  if (tft_cache)
 +
    tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
 +
  if (menu_sta == 2) {
 +
    switch (menu_num_A) {
 +
      case 0: {
 +
          switch (menu_num_B) {
 +
            case 0: {
 +
                if (tft_cache)
 +
                {
 +
                  for (uint8_t a = 0; a < 4; a++)
 +
                  {
 +
                    joy_correct_min[a] = 0;
 +
                    joy_correct_max[a] = 0;
 +
                  }
 +
                }
 +
                for (uint8_t a = 0; a < 4; a++) {
 +
                  tft.setCursor(2, 120);
 +
                  tft.print("Move Joystick MaxGear");
 +
                  int16_t _c = Joy_i(a, false, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
 +
                  if (_c > joy_correct_max[a]) {
 +
                    tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                    joy_correct_max[a] = _c;
 +
                  }
 +
                  //                  joy_correct_max[a] = constrain(joy_correct_max[a], 0, Joy_maximum);
 +
                  if (_c < joy_correct_min[a]) {
 +
                    tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                    joy_correct_min[a] = _c;
 +
                  }
 +
                  //                  joy_correct_min[a] = constrain(joy_correct_min[a], -Joy_maximum, 0);
 +
                }
 +
 +
                for (uint8_t d = 0; d < 2; d++) {
 +
                  tft.drawFastHLine(12 + 70 * d, 80, 33, tft_colorA);
 +
                  tft.drawFastVLine(28 + 70 * d, 64, 33, tft_colorA);
 +
                  //                tft.fillRect(2, 90-4, 20, 12, tft_colorB);
 +
                  tft.drawCircle(44 + 70 * d, 80, map(joy_correct_min[0 + 2 * d], 0, -512, 1, 10), tft_colorA);
 +
                  tft.drawCircle(12 + 70 * d, 80, map(joy_correct_max[0 + 2 * d], 0, 512, 1, 10), tft_colorA);
 +
                  tft.drawCircle(28 + 70 * d, 64, map(joy_correct_min[1 + 2 * d], 0, -512, 1, 10), tft_colorA);
 +
                  tft.drawCircle(28 + 70 * d, 96, map(joy_correct_max[1 + 2 * d], 0, 512, 1, 10), tft_colorA);
 +
                }
 +
                return_menu = true;
 +
              }
 +
              break;
 +
            case 1: {
 +
                if (key_get(2, 1)) {
 +
                  Joy_deadzone_val--;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
                if (key_get(3, 1)) {
 +
                  Joy_deadzone_val++;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
                Joy_deadzone_val = constrain(Joy_deadzone_val, 0, 25);
 +
 +
                TFT_menu(0, Joy_deadzone_val);
 +
                TFT_cursor(0);
 +
                return_menu = true;
 +
              }
 +
              break;
 +
          }
 +
        }
 +
        break;
 +
 +
      case 1: {
 +
          switch (menu_num_B) {
 +
            case 0: {
 +
                char *menu_str_c[2] = { "Quadro.", "nRF24"};
 +
                if (key_get(2, 1) || key_get(3, 1)) {
 +
                  mode_protocol = !mode_protocol;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
                for (uint8_t c = 0; c < 2; c++) {
 +
                  TFT_menu(c, menu_str_c[c]);
 +
                }
 +
 +
                TFT_cursor(mode_protocol);
 +
                return_menu = true;
 +
              }
 +
              break;
 +
            case 1: {
 +
#if !defined(__AVR_ATmega128RFA1__)
 +
                char *menu_str_c[5] = {"9600", "19200", "38400", "57600", "115200"};
 +
#endif
 +
                if (key_get(2, 1)) {
 +
                  mwc_channal--;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
                if (key_get(3, 1)) {
 +
                  mwc_channal++;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
 +
#if !defined(__AVR_ATmega128RFA1__)
 +
                mwc_channal = constrain(mwc_channal, 0, 4);
 +
                TFT_menu(0, menu_str_c[mwc_channal]);
 +
#else
 +
                mwc_channal = constrain(mwc_channal, 11, 26);
 +
                TFT_menu(0, mwc_channal);
 +
#endif
 +
                TFT_cursor(0);
 +
                return_menu = true;
 +
              }
 +
              break;
 +
 +
            case 2: {
 +
                if (key_get(2, 1)) {
 +
                  nrf_channal--;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
                if (key_get(3, 1)) {
 +
                  nrf_channal++;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
                nrf_channal = constrain(nrf_channal, 0, 125);
 +
 +
                TFT_menu(0, nrf_channal);
 +
                TFT_cursor(0);
 +
                return_menu = true;
 +
              }
 +
              break;
 +
          }
 +
        }
 +
        break;
 +
      case 2: {
 +
          switch (menu_num_B) {
 +
            case 0: {
 +
                tft_theme = !tft_theme;
 +
                TFT_init(true, tft_rotation);
 +
                tft_cache = 1;
 +
                tft.setTextColor(tft_colorA);
 +
                menu_sta --;
 +
              }
 +
              break;
 +
            case 1: {
 +
                tft_rotation = !tft_rotation;
 +
                TFT_init(true, tft_rotation);
 +
                tft_cache = 1;
 +
                tft.setTextColor(tft_colorA);
 +
                menu_sta --;
 +
              }
 +
              break;
 +
            case 2: {
 +
                char *menu_str_c[2] = { "3.3V", "5.0V"};
 +
                return_menu = true;
 +
 +
                if (key_get(2, 1) || key_get(3, 1)) {
 +
                  mcu_voltage = !mcu_voltage;
 +
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
 +
                }
 +
 +
                TFT_cursor(mcu_voltage);
 +
 +
                for (uint8_t c = 0; c < 2; c++) {
 +
                  TFT_menu(c, menu_str_c[c]);
 +
                }
 +
                //                tft.fillRect(0, 40, tft_width, 100,tft_colorB);
 +
              }
 +
              break;
 +
          }
 +
 +
        }
 +
        break;
 +
 +
#if !(defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__))
 +
      case 3: { //mpu
 +
          mode_mpu = menu_num_B;
 +
          tft_cache = 1;
 +
          menu_sta = 0; //back main menu
 +
          menu_num_B = 0; //zero
 +
        }
 +
        break;
 +
#endif
 +
    }
 +
  }
 +
 +
  /*
 +
    Serial.print(menu_sta);
 +
    Serial.print(",");
 +
    Serial.print(menu_num_A);
 +
    Serial.print(",");
 +
    Serial.println(menu_num_B);
 +
  */
 +
  //----------------------------
 +
  if (menu_sta == 1) {
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
    int8_t meun_b_max[5] = {1, 2, 2, 1, 0};
 +
#else
 +
    int8_t meun_b_max[4] = {1, 2, 2, 0};
 +
#endif
 +
    if (!meun_b_max[menu_num_A])
 +
      return false;
 +
    else {
 +
      if (key_get(2, 1)) {
 +
        tft.fillRect(0, 40, 5, 100, tft_colorB);
 +
        menu_num_B--;
 +
      }
 +
      if (key_get(3, 1)) {
 +
        tft.fillRect(0, 40, 5, 100, tft_colorB);
 +
        menu_num_B++;
 +
      }
 +
      menu_num_B = constrain(menu_num_B, 0, meun_b_max[menu_num_A]);
 +
 +
      TFT_cursor(menu_num_B);
 +
 +
      if (tft_cache) {
 +
        for (uint8_t b = 0; b < (meun_b_max[menu_num_A] + 1); b++) {
 +
          TFT_menu(b, menu_str_b[menu_num_A][b]);
 +
        }
 +
      }
 +
    }
 +
  }
 +
 +
  //main menu
 +
  if (menu_sta == 0) {
 +
    //custer
 +
    if (key_get(2, 1)) {
 +
      tft.fillRect(0, 40, 5, 100, tft_colorB);
 +
      menu_num_A--;
 +
    }
 +
    if (key_get(3, 1)) {
 +
      tft.fillRect(0, 40, 5, 100, tft_colorB);
 +
      menu_num_A++;
 +
    }
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
    menu_num_A = constrain(menu_num_A, 0, 4);
 +
#else
 +
    menu_num_A = constrain(menu_num_A, 0, 3);
 +
#endif
 +
 +
    TFT_cursor(menu_num_A);
 +
 +
    if (tft_cache) {
 +
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
 +
      for (uint8_t a = 0; a < 5; a++) {
 +
#else
 +
      for (uint8_t a = 0; a < 4; a++) {
 +
#endif
 +
        TFT_menu(a, menu_str_a[a]);
 +
      }
 +
    }
 +
  }
 +
 +
  if (tft_cache) {
 +
    //BACK
 +
    tft.fillCircle(12, 149, 8, tft_colorA);
 +
    tft.drawLine(11, 145, 7, 149, tft_colorB);
 +
    tft.drawLine(7, 149, 11, 153, tft_colorB);
 +
    tft.drawLine(7, 149, 17, 149, tft_colorB);
 +
    //ENTER
 +
    tft.fillCircle(12 + 20, 149, 8, tft_colorA);
 +
    tft.drawLine(10 + 20, 146, 7 + 20, 149, tft_colorB);
 +
    tft.drawLine(7 + 20, 149, 10 + 20, 152, tft_colorB);
 +
    tft.drawLine(7 + 20, 149, 15 + 20, 149, tft_colorB);
 +
    tft.drawLine(15 + 20, 146, 15 + 20, 149, tft_colorB);
 +
    //PREV
 +
    tft.fillCircle(127 - 12, 149, 8, tft_colorA);
 +
    tft.drawLine(127 - 12, 153, 127 - 8, 149, tft_colorB);
 +
    tft.drawLine(127 - 12, 153, 127 - 16, 149, tft_colorB);
 +
    tft.drawLine(127 - 12, 153, 127 - 12, 145, tft_colorB);
 +
    //NEXT
 +
    tft.fillCircle(127 - 32, 149, 8, tft_colorA);
 +
    tft.drawLine(127 - 32, 145, 127 - 28, 149, tft_colorB);
 +
    tft.drawLine(127 - 32, 145, 127 - 36, 149, tft_colorB);
 +
    tft.drawLine(127 - 32, 145, 127 - 32, 153, tft_colorB);
 +
  }
 +
  tft_cache --;
 +
  if (tft_cache < 0)  tft_cache = 0;
 +
 +
  return true;
 +
}
 +
 +
//------------------
 +
#define _C_x_S  (_Q_x + 1)
 +
#define _C_x_M  (_Q_x + ((_W_x + 1) / 2))
 +
#define _C_x_E  (_Q_x + _W_x - 1)
 +
 +
char *NAME[8] = {
 +
  "ROLL", "PITCH", "YAW", "THROT", "AUX1", "AUX2", "AUX3", "AUX4"
 +
};
 +
 +
void TFT_ready()
 +
{
 +
  tft.fillRect(0, 0, 128, 26, tft_colorA);
 +
 +
  tft.drawRect(tft_width - tft_bat_x - tft_bat_x_s - 2, 2, tft_bat_x, tft_bat_y, tft_colorB);
 +
  tft.drawRect(tft_width - tft_bat_x_s - 2, 2 + (tft_bat_y - tft_bat_y_s) / 2, tft_bat_x_s, tft_bat_y_s, tft_colorB);
 +
 +
  tft.setTextColor(tft_colorB);
 +
  setFont_S;
 +
 +
  tft.setCursor(_Q_font_x, 3);
 +
  tft.print(mode_protocol ? "nRF24" : "Quadr");
 +
  tft.print(" CHAN.");
 +
  tft.print(mode_protocol ? nrf_channal : mwc_channal);
 +
  tft.setCursor(_Q_font_x, 16);
 +
  tft.print("Time:");
 +
 +
  tft.setTextColor(tft_colorA);
 +
  for (uint8_t a = 0; a < 8; a++) {
 +
    tft.setCursor(_Q_font_x, _Q_font_y + a * 15);
 +
    tft.print(NAME[a]);
 +
    //------------------------------------------
 +
    tft.drawRect(_Q_x, _Q_y + a * 15, _W_x, _W_y, tft_colorA);
 +
  }
 +
}
 +
 +
boolean _a = false, _b = false;
 +
void TFT_run()
 +
{
 +
  if (outBuf[3] > (Joy_MID - Joy_maximum)) {
 +
    if (_a) {
 +
      Joy_time[0] = millis() - Joy_time[1];
 +
      _a = false;
 +
    }
 +
    Joy_time[1] = millis() - Joy_time[0];
 +
  }
 +
  else
 +
    _a = true;
 +
 +
  if (!_b && ((Joy_time[1] / 1000) % 2)) {
 +
    _b = !_b;
 +
    tft.fillRect(_Q_font_x + 30, 16, 50, 7, tft_colorA);
 +
    tft.setTextColor(tft_colorB);
 +
    tft.setCursor(_Q_font_x + 30, 16);
 +
    tft.print((Joy_time[1] / 1000) / 60);
 +
    tft.print("m");
 +
    tft.print((Joy_time[1] / 1000) % 60);
 +
    tft.print("s");
 +
  }
 +
  _b = boolean((Joy_time[1] / 1000) % 2);
 +
 +
  //battery------------------
 +
  tft.fillRect(tft_width - tft_bat_x - 3, 3, map(_V_bat, _V_min, _V_max, 0, tft_bat_x - 2) , tft_bat_y - 2, tft_colorB);
 +
  tft.fillRect(tft_width - tft_bat_x - 3 + map(_V_bat, _V_min, _V_max, 0, tft_bat_x - 2), 3, map(_V_bat, _V_min, _V_max, tft_bat_x - 2, 0) , tft_bat_y - 2, tft_colorA);
 +
 +
  for (uint8_t a = 0; a < 8; a++) {
 +
    int8_t _C_x_A0, _C_x_B0, _C_x_A, _C_x_B, _C_x_A1, _C_x_B1;
 +
    int8_t _C_x;
 +
 +
    if (outBuf[a] < Joy_MID) {
 +
      _C_x = map(outBuf[a], Joy_MID - Joy_maximum, Joy_MID, _C_x_S, _C_x_M);
 +
 +
      _C_x_A0 = _C_x_S;
 +
      _C_x_B0 = _C_x - _C_x_S;
 +
 +
      _C_x_A = _C_x;
 +
      _C_x_B = _C_x_M - _C_x;
 +
 +
      _C_x_A1 = _C_x_M;
 +
      _C_x_B1 = _C_x_E - _C_x_M;
 +
    } else if (outBuf[a] > Joy_MID) {
 +
      _C_x = map(outBuf[a], Joy_MID, Joy_MID + Joy_maximum, _C_x_M, _C_x_E);
 +
 +
      _C_x_A0 = _C_x_S;
 +
      _C_x_B0 = _C_x_M - _C_x_S;
 +
 +
      _C_x_A = _C_x_M;
 +
      _C_x_B = _C_x - _C_x_M;
 +
 +
      _C_x_A1 = _C_x;
 +
      _C_x_B1 = _C_x_E - _C_x;
 +
    } else {
 +
      _C_x_A0 = _C_x_S;
 +
      _C_x_B0 = _C_x_M - _C_x_S;
 +
 +
      _C_x_A = _C_x_M;
 +
      _C_x_B = 0;
 +
 +
      _C_x_A1 = _C_x_M;
 +
      _C_x_B1 = _C_x_E - _C_x_M;
 +
    }
 +
    tft.fillRect(_C_x_A0,  _Q_y + a * 15 + 1, _C_x_B0, _W_y - 2, tft_colorB);
 +
    tft.fillRect(_C_x_A,  _Q_y + a * 15 + 1, _C_x_B, _W_y - 2, tft_colorC);
 +
    tft.fillRect(_C_x_A1,  _Q_y + a * 15 + 1, _C_x_B1, _W_y - 2, tft_colorB);
 +
 +
    tft.fillRect(_C_x_M,  _Q_y + a * 15 - 1, 1, _W_y + 2, tft_colorD);
 +
  }
 +
  //netsta------------------
 +
  tft.fillRect(0, 158, 128, 2, node_clock_error ? tft_colorD : tft_colorC);
 +
}
 +
</source>
 +
time.h
 +
<source lang="cpp">
 +
#include "Arduino.h"
 +
 +
//unsigned long time;
 +
unsigned long TIME1;            //setup delay
 +
unsigned long time2; //send data
 +
unsigned long time3; //battery
 +
unsigned long Joy_time[2] = {0, 0}; //joy
 +
</source>
  
 
==视频==
 
==视频==

2015年11月16日 (一) 11:50的版本

概述

  • 项目名称：Microduino自平衡车
  • 目的：制作一台可以用遥控板控制的自平衡机器人小车
  • 难度：中
  • 耗时：2小时
  • 制作者：
  • 简介:

两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本次教程我们使用Microduino产品模块快速搭建一个可以用遥控板控制的自平衡机器人小车，玩家可以迅速上手，并且看到小车运动和平衡的效果，玩家们可以在制作结束后，继续更深一步的智能控制部分的研究。

材料清单

  • Microduino设备
模块 数量 功能
Microduino-Core+/zh 1 核心板
Microduino-USBTTL/zh 1 下载程序
Microduino-nRF24/zh 1 无线通信
Microduino-Robot/zh 1 驱动连接地板
Microduino-Stepper/zh 2 驱动步进电机
  • 其他设备
模块 数量 功能
2.4G天线 1 2.4G通讯
固定支架 1 固定支撑
尼龙螺丝 4 固定
尼龙螺母 12 固定
电池盒 1 装载电池
电池 2 供电
Micro-USB数据线 1 串口通信，下载程序
车轴 2 连接车轮
车轮子 2 结构
步进电机 2 驱动车轮
  • Joypad材料
模块 数量 功能
Microduino-Core 1 核心
Microduino-nRF24 1 无线通讯
Microduino-Joypad 1 遥控器
Microduino-TFT 1 显示
Microduino-USBTTL 1 下载程序
锂电池 1 供电
亚克力面板 1 面板
尼龙柱 4 固定
长尼龙螺丝 4 固定
短尼龙螺丝 4 固定
尼龙螺母 4 固定
海绵板 1 固定
文件:平衡车物料.jpg
600px

实验原理

  • PID原理

PID在自动控制领域有着极其重要的作用，作为最早实用化的控制技术已经有70多年的历史，近几年一些创客们自制的一些很酷的东西，如：四轴飞行器，自平衡车等等都离不开它的身影。 首先了解什么是PID。PID实指“比例proportional”、“积分integral”、“微分derivative”，如果我们要求被控制的对象最终趋于一个稳定的结果，一般就可以使用PID算法。 假设说，有一辆速度为1m/s的小车，我们要求他的速度改变为5m/s，要完成这样的一件事，我们必须要有：

  • 小车驱动装置（用程序控制它输出多大的电压，电压决定驱动的马力）；
  • 被驱动器控制的部分（即小车）；
  • 检测当前速度的装置（当前速度与目标速度的差称为误差）；

本来，我们可以给小车一个驱动力让小车加速，直到检测到小车速度达到5m/s，撤去驱动力。但是，这样做会带来几个问题： 1)当小车速度达到5m/s时，从装置检测到这个速度，通知控制器，让控制器改变输出的电压，这一个过程需要耗费一定时间，在这个时间里面，小车速度可能增加了不少。 2)撤去驱动力后，外界条件如摩擦会让小车速度进一步改变。 然而，PID算法可以在一定误差内解决这些问题。 使用PID算法时，大致是这样的。每一个采样周期，通过速度检测装置获得当前速度，传入程序，通过程序计算得到电压控制小车得到新速度。下一个采样周期又把新速度传入，获得新电压，再传入速度，再获得电压，如此反复。 PID算法的关键，是如何根据当前得到的速度值，输出一个“恰当”的电压，以致小车最终能够趋于稳定。 PID算法采用比例，积分，微分(Proportion Integral Differential)三种方法进行控制。三种方法都有自己对应的一个常量(pconst，iconst，dconst)。这三个变量都需要在实验中多次尝试得出。用数学公式表达PID算法如下图所示： 此处：e = 期望值 – 输入值

文件:PIDtheory.jpg
600px

平衡车之所以可以自己掌握平衡，首先通过Microduino-10DOF模块的加速度和陀螺仪测出相应的姿态数据，然后利用kalman滤波算法得出当前平衡车的角度。平衡的角度是180度，如果测出的角度偏离180，PID算法会调整输出相应的PWM值给电机，从而保持小车平衡。 PID原理有点像锅炉房里烧锅炉，首先定下来锅炉的恒定温度，比如26摄氏度，锅炉房里的墙上有一个温度计，能够实时测得锅炉的实时温度。锅炉房里通常有个老大爷时不时（每十分钟看一次）的看着墙上的温度计，如果温度高了就给锅炉降温，低了就给锅炉升温。如果让一个没有经验的年轻小伙子来管理锅炉的温度，可以想象温度表的值会上下浮动的，有经验的老大爷会把这个浮动降到最低。其实PID就是这个烧锅炉的例子，在代码中就有这个故事的影子： 1)规定的26度就是setpoint 2)当前的温度就是CurrentAngle 3)实际值与26度的偏差就是error 4)没有经验的小伙子有点像PID中的P 5)有经验的老大爷相当于PID了 6)每十分钟看一次相当于PID计算的周期时间 PID的主要代码： 

// PD的实施。 DT是毫秒 
float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd) 
{ 
    float error; 
    float output; 
    error = setPoint - input; 
    // Kd的两部分实施 
    //仅使用输入（传感器）的一部分而不是设定值输入（T-2）最大的一个 
    //而第二个使用该设定值 
    output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; 
    //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t"); 
    PID_errorOld2 = PID_errorOld; 
    PID_errorOld = input;  // 误差为Kd值是唯一的输入组件 
    setPointOld = setPoint; 
    return(output); 
} 
//P控制实现。 
float speedPControl(float input, float setPoint,  float Kp) 
{ 
    float error; 
    error = setPoint - input; 
    return(Kp * error); 
} 

本套件利用陀螺仪和加速度传感器（Microduino-10DOF/zh）来检测车体态的变化，并利用步进电机控制核心（Microduino-Stepper/zh），精确地驱动电机进行相应的调整，以保持系统的平衡。

文件:PIDtheory1.jpg
600px
  • 主要传感器

Microduino-10DOF

文档

调试过程

将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），通过USB数据线与电脑连接起来

Download1.jpg

确认你搭建了Microduino的开发环境，否则参考附录1-Arduino IDE安装指导。

文件:Gettingstarted.jpg
600px

打开Arduino IDE编程软件，点击 【文件】->【打开】

Dl1.jpg

浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开

文件:Balancecaropen1.jpg
600px
文件:Balancecaropen2.jpg
600px

点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序

文件:WiFiStationopen4.jpg
600px
  • 平衡车骨架搭建
文件:平衡车步骤.jpg
600px

完成这一步平衡车就搭建完成了

  • Joypad遥控板的搭建与调试

将Microduino-Core与Microduino-USBTTL叠加（无上下顺序），通过USB数据与电脑连接起来。

Download1.jpg

打开Arduino IDE编程软件，点击【文件】->【打开】

Dl1.jpg

浏览到项目程序地址，点击“Joypad_RC_2.3.ino”程序打开。

文件:Joypadopen.jpg
300px
文件:Joypadopen1.jpg
300px

点击【工具】，在板选项里面选择板卡（Microduino-Core），在处理器选项里面选择处理器（Atmega328p@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序。

Dl3.jpg
  • 安装Joypad
文件:Joypad物料.jpg
300px
文件:Joypad步骤.jpg
300px
  • 自平衡小车和Joypad测试

Joypad操作说明

文件:Joypadtest.jpg
300px
  • 左上边是油门控制开关，打开（拨到上面），才能进行控制，你可以摇动摇杆，观察屏幕的变化。
  • 右边开关是精度调整开关，开关拨到上面可以最大幅度控制，否则只能小幅度控制了，小幅度有助于稳定控制。
  • 左边摇杆本次未使用。
  • 右边摇杆在垂直方向上控制前后方向移动，往上向前，往下向后，在水平方向上控制左右方向移动。

Joypad开机设置

文件:Joypadtest1.jpg
300px

打开遥控器电源开关，按下复位按键（左边USB接口右边那个）进入系统，请在4S内按下【key1】按键，进入遥控器校准和控制选择模式。 360度最大幅度旋转两个摇杆，遥控板会读入摇杆的位置数据，摇动至示数不再变化即可

文件:Joypadtest2.jpg
300px

选择控制模式，可以通过【key3】按键来选择是控制四轴飞行器（Quad.）还是机器人（Robot），Robot模式可控制自平衡车和BOXZ mini，黑色表示选中。因此我们需要选择Robot模式。还可以通过【key4】按键来选择是否是体感控制模式，如果选择体感模式，你必须叠加Microduino-10DOF模块，选择“MPU ON”。如果是摇杆控制模式，选择“MPU OFF”。 这次搭建没有使用10DOF模块，因此选择MPU OFF模式；

文件:Joypadtest3.jpg
300px

选择完成后，通过【key2】按键退出配置，进入操作

文件:Joypadtest4.jpg
300px

将左上边控制开关打开（拨到上面），才能进行控制，你可以摇动摇杆，观察屏幕的变化

文件:Joypadtest5.jpg
300px

右边开关是幅度调节模式，开关拨到上面可以最大幅度控制Robot，否则只能小幅度控制。如果使用小幅度控制小车，右边摇杆拨到最大位置，小车速度也只能小范围变化，这样有助于稳定控制

文件:Joypadtest6.jpg
300px

当启动小车时，只需要用到右边的摇杆，摇杆的方向和平衡车的方向一致，你可以尝试摇杆控制是否正确。 测试通过后，就可以打开平衡车上Microduino-Robot底板上的电源开关，拨到ON（左边），如果可以看到核心板上的红色led亮，说明供电正常。这样制作就完成了，可以愉快的玩耍了

文件:Joypadtest7.jpg
300px

注意问题

  • 下载程序时候最好只叠加core（core+）和USBTTL，虽然本次搭建涉及的nRF24不会引起冲突，但是别的通信模块有时会造成串口冲突，养成好习惯。
  • Core+要叠在nRF24，USB的底下，紧贴ROBOT板。
  • 锂电池正负极别接错了，否则会烧坏电路。
  • 调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池

程序说明

Joypad程序及说明

Joypad_RC.ino 

#include "Arduino.h"
#include "def.h"
#include "time.h"
#include "bat.h"
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
#include "mpu.h"
#endif
#include "joy.h"
#include "key.h"
#include "data.h"
#include "nrf.h"
#include "mwc.h"
#include "tft.h"
#include "eep.h"

#if defined(__AVR_ATmega128RFA1__)
#include <ZigduinoRadio.h>
#endif

//joypad================================
#include <Joypad.h>
//eeprom================================
#include <EEPROM.h>
//TFT===================================
#include <Adafruit_GFX.h>    // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific 
#include <SPI.h>
//rf====================================
#include <RF24Network.h>
#include <RF24.h>

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
//MPU===================================
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#endif

//spi===================================
#include <SPI.h>

void setup()
{
  // initialize serial communication at 115200 bits per second:

#ifdef Serial_DEBUG
  Serial.begin(115200);
  delay(100);
  Serial.println("========hello========");
#endif

  //---------------
  key_init();

  //---------------
#ifdef Serial_DEBUG
  Serial.println("\n\r EEPROM READ...");
#endif
  eeprom_read();

  //---------------
#ifdef Serial_DEBUG
  Serial.println("\n\r TFT INIT...");
#endif
  TFT_init(true, tft_rotation);

  //---------------
#ifdef Serial_DEBUG
  Serial.println("\n\r TFT BEGIN...");
#endif
  TIME1 = millis();
  while (millis() - TIME1 < interval_TIME1)
  {
    TFT_begin();

    if (!Joypad.readButton(CH_SWITCH_1))
    {
#ifdef Serial_DEBUG
      Serial.println("\n\rCorrect IN...");
#endif

      //---------------
#ifdef Serial_DEBUG
      Serial.println("\n\r TFT INIT...");
#endif
      TFT_init(false, tft_rotation);

      while (1)
      {
        if (!TFT_config())
          break;
      }
#ifdef Serial_DEBUG
      Serial.println("\n\rCorrect OUT...");
#endif

      //---------------
#ifdef Serial_DEBUG
      Serial.println("\n\r EEPROM WRITE...");
#endif
      eeprom_write();
    }
  }

  //---------------
#ifdef Serial_DEBUG
  Serial.println("\n\r TFT CLEAR...");
#endif
  TFT_clear();

  //---------------
#ifdef Serial_DEBUG
  Serial.println("\n\r TFT READY...");
#endif
  TFT_ready();

  //---------------.l
  if (mode_protocol)   //Robot
  {
    SPI.begin();		//初始化SPI总线
    radio.begin();
    network.begin(/*channel*/ nrf_channal, /*node address*/ this_node);
  }
  else          //QuadCopter
  {
    unsigned long _channel;
#if !defined(__AVR_ATmega128RFA1__)
    switch (mwc_channal)
    {
      case 0:
        _channel = 9600;
        break;
      case 1:
        _channel = 19200;
        break;
      case 2:
        _channel = 38400;
        break;
      case 3:
        _channel = 57600;
        break;
      case 4:
        _channel = 115200;
        break;
    }
#else if
    _channel = mwc_channal;
#endif
    mwc_port.begin(_channel);
  }

  //---------------
#ifdef Serial_DEBUG
  Serial.println("===========start===========");
#endif

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
  if (mode_mpu) initMPU(); //initialize device
#endif
}

void loop()
{
  //  unsigned long time = millis();

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
  //MPU--------------------------------
  if (mode_mpu)
    getMPU();
#endif

  //DATA_begin------------------------------
  data_begin();

  //DATA_send-------------------------------
  if (millis() < time2) time2 = millis();
  if (millis() - time2 > interval_time2)
  {
    if (mode_protocol) nrf_send();    //Robot
    else data_send();           //QuadCopter

    time2 = millis();
  }

  //节点查错-------------------------------
  vodebug();

  //BAT--------------------------------
  if (time3 > millis()) time3 = millis();
  if (millis() - time3 > interval_time3)
  {
    vobat();
    time3 = millis();
  }

  //TFT------------------------------------
  TFT_run();

  //===================================
  //  time = millis() - time;

  //  Serial.println(time, DEC);    //loop time
}
</cpp>
BAT.h
<source lang="cpp">
int8_t _V_bat = _V_min;

boolean mcu_voltage = true; // 5.0 or 3.3
#define _V_fix 0.2  //fix battery voltage
#define _V_math(Y) (_V_fix+((Y*analogRead(PIN_bat)/1023.0f)/(33.0f/(51.0f+33.0f))))

void vobat()
{
  //_V_bat=10*((voltage*analogRead(PIN_bat)/1023.0f)/(33.0f/(51.0f+33.0f)));
  _V_bat = _V_math(mcu_voltage ? 50 : 33);
  _V_bat = constrain(_V_bat, _V_min, _V_max);

#ifdef Serial_DEBUG
  Serial.print("_V_bat: ");
  Serial.println(_V_bat);
#endif
}

data.h 

#include "Arduino.h"

byte inBuf[16];

int16_t outBuf[8] =
{
  Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID
};

boolean AUX[4] = {0, 0, 0, 0};
//======================================
void data_begin()
{
  Joy();

  if (mode_protocol)   //Robot
  {
    if (!sw_l)
    {
      Joy_x = Joy_MID;
      Joy_y = Joy_MID;
      Joy1_x = Joy_MID;
      Joy1_y = Joy_MID;
    }
  }
  else        //QuadCopter
  {
    if (!sw_l)
      Joy_y = Joy_MID - Joy_maximum;
  }

  //but---------------------------------
  for (uint8_t a = 0; a < 4; a++)
  {
    if (key_get(a, 1))  AUX[a] = !AUX[a];
  }

  outBuf[0] = Joy1_x;
  outBuf[1] = Joy1_y;
  outBuf[2] = Joy_x;
  outBuf[3] = Joy_y;
  outBuf[4] = map(AUX[0], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
  outBuf[5] = map(AUX[1], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
  outBuf[6] = map(AUX[2], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
  outBuf[7] = map(AUX[3], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
}

def.h 

#include "Arduino.h"

//DEBUG-----------
#define Serial_DEBUG

//MWC-------------
uint8_t mwc_channal = 11; //RF channel

#if  defined(__AVR_ATmega32U4__)
#define mwc_port Serial1    //Serial1 is D0 D1
#elif defined(__AVR_ATmega128RFA1__)
#define mwc_port ZigduinoRadio    //RF
#else
#define mwc_port Serial    //Serial is D0 D1
#endif

//nRF-------------
#define interval_debug  2000  //节点查错间隔
uint8_t nrf_channal = 70;  //0~125

//Battery---------
#define PIN_bat A7	//BAT

#define _V_max 41		//锂电池满电电压4.2V
#define _V_min 36		//锂电池没电电压3.7V

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
//MPU-------------
#define MPU_maximum 70
#endif


//Time------------
#define interval_TIME1 2000    //setup delay
#define interval_time2 40      //send interval
#define interval_time3 1000    //battery interval

eep.h 

#include "Arduino.h"

#include <EEPROM.h>

#define EEPROM_write(address, p) {int i = 0; byte *pp = (byte*)&(p);for(; i < sizeof(p); i++) EEPROM.write(address+i, pp[i]);}
#define EEPROM_read(address, p)  {int i = 0; byte *pp = (byte*)&(p);for(; i < sizeof(p); i++) pp[i]=EEPROM.read(address+i);}

struct config_type
{
  int16_t eeprom_correct_min[4];
  int16_t eeprom_correct_max[4];
  uint8_t eeprom_Joy_deadzone_val;
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
  boolean eeprom_mode_mpu;
#endif
  boolean eeprom_mode_protocol;
  uint8_t eeprom_mwc_channal;
  uint8_t eeprom_nrf_channal;
  boolean eeprom_tft_theme;
  boolean eeprom_tft_rotation;
  boolean eeprom_mcu_voltage;
};

//======================================
void eeprom_read()
{
  //EEPROM读取赋值
  config_type config_readback;
  EEPROM_read(0, config_readback);

  for (uint8_t a = 0; a < 4; a++)
  {
    joy_correct_min[a] = config_readback.eeprom_correct_min[a];
    joy_correct_max[a] = config_readback.eeprom_correct_max[a];
  }
  Joy_deadzone_val = config_readback.eeprom_Joy_deadzone_val;

  mode_protocol = config_readback.eeprom_mode_protocol;
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
  mode_mpu = config_readback.eeprom_mode_mpu;
#endif

  mwc_channal = config_readback.eeprom_mwc_channal;
  nrf_channal = config_readback.eeprom_nrf_channal;
  tft_theme = config_readback.eeprom_tft_theme;
  tft_rotation = config_readback.eeprom_tft_rotation;
  mcu_voltage = config_readback.eeprom_mcu_voltage;
}

void eeprom_write()
{
  // 定义结构变量config，并定义config的内容
  config_type config;

  for (uint8_t a = 0; a < 4; a++)
  {
    config.eeprom_correct_min[a] = joy_correct_min[a];
    config.eeprom_correct_max[a] = joy_correct_max[a];
  }
  config.eeprom_Joy_deadzone_val = Joy_deadzone_val;

  config.eeprom_mode_protocol = mode_protocol;
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
  config.eeprom_mode_mpu = mode_mpu;
#endif

  config.eeprom_mwc_channal = mwc_channal;
  config.eeprom_nrf_channal = nrf_channal;
  config.eeprom_tft_theme = tft_theme;
  config.eeprom_tft_rotation = tft_rotation;
  config.eeprom_mcu_voltage = mcu_voltage;

  // 变量config存储到EEPROM，地址0写入
  EEPROM_write(0, config);
}

joy.h 

#include "Arduino.h"

#include <Joypad.h>

//Joy-------------
//1000~2000
uint8_t Joy_deadzone_val = 10;
#define Joy_s_maximum 200 //MAX 300
#define Joy_maximum 450 //MAX 500
#define Joy_MID 1500  //1500

boolean mode_mpu, mode_protocol;   //{(0: 0 is mwc, 1 is nrf),(1: 0 is mpu, 1 is no mpu)}

int16_t joy_correct_max[4], joy_correct_min[4];
int16_t Joy_x, Joy_y, Joy1_x, Joy1_y;

int16_t s_lig, s_mic;

boolean Joy_sw, Joy1_sw;

boolean but1, but2, but3, but4;

boolean sw_l, sw_r;

//======================================
int16_t Joy_dead_zone(int16_t _Joy_vol)
{
  if (abs(_Joy_vol) > Joy_deadzone_val)
    return ((_Joy_vol > 0) ? (_Joy_vol - Joy_deadzone_val) : (_Joy_vol + Joy_deadzone_val));
  else
    return 0;
}

int16_t Joy_i(int16_t _Joy_i, boolean _Joy_b, int16_t _Joy_MIN, int16_t _Joy_MAX)
{
  int16_t _Joy_a;
  switch (_Joy_i)
  {
    case 0:
      _Joy_a = Joy_dead_zone(Joypad.readJoystickX());
      break;
    case 1:
      _Joy_a = Joypad.readJoystickY();    //throt
      break;
    case 2:
      _Joy_a = Joy_dead_zone(Joypad.readJoystick1X());
      break;
    case 3:
      _Joy_a = Joy_dead_zone(Joypad.readJoystick1Y());
      break;
  }

  if (_Joy_b)
  {
    if (_Joy_a < 0)
      _Joy_a = map(_Joy_a, joy_correct_min[_Joy_i], 0, _Joy_MAX, Joy_MID);
    else
      _Joy_a = map(_Joy_a, 0, joy_correct_max[_Joy_i], Joy_MID, _Joy_MIN);

    if (_Joy_a < _Joy_MIN) _Joy_a = _Joy_MIN;
    if (_Joy_a > _Joy_MAX) _Joy_a = _Joy_MAX;
  }
  return _Joy_a;
}

void Joy()
{
  sw_l = Joypad.readButton(CH_SWITCH_L);
  sw_r = Joypad.readButton(CH_SWITCH_R);

  //------------------------------------
  //s_lig=Joypad.readLightSensor();
  //s_mic=Joypad.readMicrophone();

  //------------------------------------
  Joy_sw = Joypad.readButton(CH_JOYSTICK_SW);
  Joy1_sw = Joypad.readButton(CH_JOYSTICK1_SW);

  //------------------------------------
  but1 = Joypad.readButton(CH_SWITCH_1);
  but2 = Joypad.readButton(CH_SWITCH_2);
  but3 = Joypad.readButton(CH_SWITCH_3);
  but4 = Joypad.readButton(CH_SWITCH_4);

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
  //====================================
  int16_t y[3];        //MPU---------------------------------
  if (mode_mpu)     //MPU---------------------------------
  {
    for (uint8_t a = 0; a < 3; a++)
    {
      y[a] = ypr[a] * 180 / M_PI;
      if (y[a] > MPU_maximum) y[a] = MPU_maximum;
      if (y[a] < -MPU_maximum) y[a] = -MPU_maximum;
    }
  }
#endif

  if (Joypad.readButton(CH_SWITCH_R))
  {
    Joy_x = Joy_i(0, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
    Joy_y = Joy_i(1, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
    if (mode_mpu)     //MPU---------------------------------
    {
      Joy1_x = map(y[2], -MPU_maximum, MPU_maximum, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
      Joy1_y = map(y[1], -MPU_maximum, MPU_maximum, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
    }
    else
#endif
    {
      Joy1_x = Joy_i(2, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
      Joy1_y = Joy_i(3, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
    }
  }
  else
  {
    Joy_x = Joy_i(0, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
    Joy_y = Joy_i(1, true, mode_protocol ? Joy_MID - Joy_s_maximum : Joy_MID - Joy_maximum, mode_protocol ? Joy_MID + Joy_s_maximum : Joy_MID + Joy_maximum); //  Robot,QuadCopter

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
    if (mode_mpu)     //MPU---------------------------------
    {
      Joy1_x = map(y[2], -MPU_maximum, MPU_maximum, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
      Joy1_y = map(y[1], -MPU_maximum, MPU_maximum, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
    }
    else
#endif
    {
      Joy1_x = Joy_i(2, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
      Joy1_y = Joy_i(3, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum);
    }
  }
}

key.h 

#include "arduino.h"

uint8_t key_pin[4] = {CH_SWITCH_1, CH_SWITCH_2, CH_SWITCH_3, CH_SWITCH_4}; //按键1 2 3 4

boolean key_status[4];			//按键
boolean key_cache[4];		//检测按键松开缓存

void key_init()
{
  for (uint8_t a = 0; a < 4; a++)
  {
    key_status[a] = LOW;
    key_cache[a] = HIGH;
  }
}

boolean key_get(uint8_t _key_num, boolean _key_type)
{
  key_cache[_key_num] = key_status[_key_num];		//缓存作判断用

  key_status[_key_num] = !Joypad.readButton(key_pin[_key_num]);	//触发时

  switch (_key_type)
  {
    case 0:
      if (!key_status[_key_num] && key_cache[_key_num])		//按下松开后
        return true;
      else
        return false;
      break;
    case 1:
      if (key_status[_key_num] && !key_cache[_key_num])		//按下松开后
        return true;
      else
        return false;
      break;
  }
}

mpu.h 

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu;

//MPU-------------
#define MPU_maximum 70

// MPU control/status vars
boolean dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

void initMPU()
{
  Wire.begin();
#ifdef Serial_DEBUG
  Serial.println(F("Initializing I2C devices..."));
#endif
  mpu.initialize();
  // verify connection
#ifdef Serial_DEBUG
  Serial.println(F("Testing device connections..."));
#endif
  if (mpu.testConnection())
  {
#ifdef Serial_DEBUG
    Serial.println("MPU6050 connection successful");
#endif
  }
#ifdef Serial_DEBUG
  else
    Serial.println(F("MPU6050 connection failed"));
#endif

  // load and configure the DMP
#ifdef Serial_DEBUG
  Serial.println(F("Initializing DMP..."));
#endif
  devStatus = mpu.dmpInitialize();

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
#ifdef Serial_DEBUG
    Serial.println(F("Enabling DMP..."));
#endif
    mpu.setDMPEnabled(true);

    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    //    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
#ifdef Serial_DEBUG
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
#endif
  }
}

void getMPU()
{
  if (!dmpReady) return;
  {
    // reset interrupt flag and get INT_STATUS byte
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
      // reset so we can continue cleanly
      mpu.resetFIFO();
#ifdef Serial_DEBUG
      Serial.println(F("FIFO overflow!"));
#endif
      // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
      // wait for correct available data length, should be a VERY short wait
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      // read a packet from FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);

      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount -= packetSize;

      // display ypr angles in degrees
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

      //Serial.print("ypr\t");
      //Serial.print(ypr[0] * 180/M_PI);
      //Serial.print("\t");
      //Serial.print(ypr[1] * 180/M_PI);
      //Serial.print("\t");
      // Serial.println(ypr[2] * 180/M_PI);
    }
  }
}

#endif

mwc.h 

#include "Arduino.h"

#if defined(__AVR_ATmega128RFA1__)
#include <ZigduinoRadio.h>
#endif

int16_t RCin[8], RCoutA[8], RCoutB[8];

int16_t p;
uint16_t read16()
{
  uint16_t r = (inBuf[p++] & 0xFF);
  r += (inBuf[p++] & 0xFF) << 8;
  return r;
}

uint16_t t, t1, t2;
uint16_t write16(boolean a)
{
  if (a)
  {
    t1 = outBuf[p++] >> 8;
    t2 = outBuf[p - 1] - (t1 << 8);
    t = t1;
  }
  else
    t = t2;
  return t;
}

typedef  unsigned char byte;
byte getChecksum(byte length, byte cmd, byte mydata[])
{
  //三个参数分别为： 数据长度  ，  指令代码  ，  实际数据数组
  byte checksum = 0;
  checksum ^= (length & 0xFF);
  checksum ^= (cmd & 0xFF);
  for (uint8_t i = 0; i < length; i++)
  {
    checksum ^= (mydata[i] & 0xFF);
  }
  return checksum;
}

void data_rx()
{
  //  s_struct_w((int*)&inBuf,16);
  p = 0;
  for (uint8_t i = 0; i < 8; i++)
  {
    RCin[i] = read16();
    /*
    Serial.print("RC[");
     Serial.print(i+1);
     Serial.print("]:");

     Serial.print(inBuf[2*i],DEC);
     Serial.print(",");
     Serial.print(inBuf[2*i+1],DEC);

     Serial.print("---");
     Serial.println(RCin[i]);
     */
    //    delay(50);        // delay in between reads for stability
  }
}

void data_tx()
{
  p = 0;
  for (uint8_t i = 0; i < 8; i++)
  {
    RCoutA[i] = write16(1);
    RCoutB[i] = write16(0);

    /*
    Serial.print("RC[");
     Serial.print(i+1);
     Serial.print("]:");

     Serial.print(RCout[i]);

     Serial.print("---");

     Serial.print(RCoutA[i],DEC);
     Serial.print(",");
     Serial.print(RCoutB[i],DEC);

     Serial.println("");
     */
    //    delay(50);        // delay in between reads for stability
  }
}

/*
if Core RF
[head,2byte,0xAA 0xBB] [type,1byte,0xCC] [data,16byte] [body,1byte(from getChecksum())]
 Example:
 AA BB CC 1A 01 1A 01 1A 01 2A 01 3A 01 4A 01 5A 01 6A 01 0D **
 */
void data_send()
{
  data_tx();

#if !defined(__AVR_ATmega128RFA1__)
  static byte buf_head[3];
  buf_head[0] = 0x24;
  buf_head[1] = 0x4D;
  buf_head[2] = 0x3C;
#endif

#define buf_length 0x10   //16
#define buf_code 0xC8     //200

  static byte buf_data[buf_length];
  for (uint8_t a = 0; a < (buf_length / 2); a++)
  {
    buf_data[2 * a] = RCoutB[a];
    buf_data[2 * a + 1] = RCoutA[a];
  }

  static byte buf_body;
  buf_body = getChecksum(buf_length, buf_code, buf_data);

  //----------------------
#if defined(__AVR_ATmega128RFA1__)
  mwc_port.beginTransmission();
  mwc_port.write(0xaa);
  mwc_port.write(0xbb);
  mwc_port.write(0xcc);
#else
  for (uint8_t a = 0; a < 3; a++) {
    mwc_port.write(buf_head[a]);
  }
  mwc_port.write(buf_length);
  mwc_port.write(buf_code);
#endif
  for (uint8_t a = 0; a < buf_length; a++) {
    mwc_port.write(buf_data[a]);
  }
  mwc_port.write(buf_body);
#if defined(__AVR_ATmega128RFA1__)
  mwc_port.endTransmission();
#endif
}

nrf.h 

#include "Arduino.h"

#include <RF24Network.h>
#include <RF24.h>
#include <SPI.h>

// nRF24L01(+) radio attached using Getting Started board
RF24 radio(9, 10);   //ce,cs
RF24Network network(radio);

#define this_node  0	//设置本机ID
#define other_node 1

//--------------------------------
struct send_a	//发送
{
  uint32_t ms;
  uint16_t rf_CH0;
  uint16_t rf_CH1;
  uint16_t rf_CH2;
  uint16_t rf_CH3;
  uint16_t rf_CH4;
  uint16_t rf_CH5;
  uint16_t rf_CH6;
  uint16_t rf_CH7;
};

struct receive_a	//接收
{
  uint32_t node_ms;
};

//--------------------------------
unsigned long node_clock, node_clock_debug, node_clock_cache = 0;		//节点运行时间、节点响应检查时间、节点时间缓存

//debug--------------------------
boolean node_clock_error = false;	//节点响应状态
unsigned long time_debug = 0;		//定时器


//======================================
void vodebug()
{
  if (millis() - time_debug > interval_debug)
  {
    node_clock_error = boolean(node_clock == node_clock_debug);		//一定时间内，节点返回的运行时间若不变则有问题

    node_clock_debug = node_clock;

    time_debug = millis();
  }
}


void nrf_send()
{
#ifdef Serial_DEBUG
  Serial.print("Sending...");
#endif

  send_a sen = {
    millis(), outBuf[0], outBuf[1], outBuf[2], outBuf[3], outBuf[4], outBuf[5], outBuf[6], outBuf[7]
  };		//把这些数据发送出去，对应前面的发送数组
  RF24NetworkHeader header(other_node);
  if (network.write(header, &sen, sizeof(sen)))
  {
#ifdef Serial_DEBUG
    Serial.print("Is ok.");
#endif

    delay(50);
    network.update();
    // If it's time to send a message, send it!
    while ( network.available() )
    {
      // If so, grab it and print it out
      RF24NetworkHeader header;
      receive_a rec;
      network.read(header, &rec, sizeof(rec));

      node_clock = rec.node_ms;		//运行时间赋值
    }
  }
#ifdef Serial_DEBUG
  else
    Serial.print("Is failed.");

  Serial.println("");
#endif
}

tft.h 

#include "Arduino.h"

#include <Adafruit_GFX.h>    // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library
#include <SPI.h>

Adafruit_ST7735 tft = Adafruit_ST7735(5, 4, -1);    //cs,dc,rst
//-------字体设置，大、中、小
#define setFont_M tft.setTextSize(2)
#define setFont_S tft.setTextSize(0)

#define tft_width  128
#define tft_height 160

boolean tft_theme = false;  //0 is white,1 is black
boolean tft_rotation = 1;

#define TFT_TOP ST7735_BLACK
#define TFT_BUT ST7735_WHITE

uint16_t  tft_colorA = TFT_BUT;
uint16_t  tft_colorB = TFT_TOP;
uint16_t  tft_colorC = 0x06FF;
uint16_t  tft_colorD = 0xEABF;

#define tft_bat_x 24
#define tft_bat_y 12
#define tft_bat_x_s 2
#define tft_bat_y_s 6

#define tft_font_s_height 8
#define tft_font_m_height 16
#define tft_font_l_height 24

#define _Q_x 33
#define _Q_y 36
#define _W_x 93
#define _W_y 5

#define _Q_font_x 2
#define _Q_font_y (_Q_y - 1)

int8_t tft_cache = 1;

//======================================
void TFT_clear()
{
  tft.fillScreen(tft_colorB);
}

void TFT_init(boolean _init, boolean _rot)
{
  tft_colorB = tft_theme ? TFT_TOP : TFT_BUT;
  tft_colorA = tft_theme ? TFT_BUT : TFT_TOP;

  if (_init) {
    tft.initR(INITR_BLACKTAB);   // initialize a ST7735S chip, black tab
    //  Serial.println("init");
    tft.fillScreen(tft_colorB);

    if (_rot)
      tft.setRotation(2);
  }

  tft.fillRect(0, 0, tft_width, 40, tft_colorA);
  tft.setTextColor(tft_colorB);
  setFont_M;
  tft.setCursor(26, 6);
  tft.print("Joypad");
  setFont_S;
  tft.setCursor(32, 24);
  tft.print("Microduino");
  tft.fillRect(0, 40, tft_width, 120, tft_colorB);
}

void TFT_begin()
{
  setFont_S;

  tft.setTextColor(tft_colorA);
  tft.setCursor(_Q_font_x, 44);
  tft.println("[key1] enter config");

  setFont_M;
  tft.setCursor(4, 150);
  for (uint8_t a = 0; a < (millis() - TIME1) / (interval_TIME1 / 10); a++) {
    tft.print("-");
  }
}

int8_t menu_num_A = 0;
int8_t menu_num_B = 0;
int8_t menu_sta = 0;

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
char *menu_str_a[5] = {
  "Joystick Config", "Protocol Config", "System Config", "Gyroscope Config", "Exit"
};
#else
char *menu_str_a[4] = {
  "Joystick Config", "Protocol Config", "System Config", "Exit"
};
#endif

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
char *menu_str_b[4][3] = {
  {"Joystick Correct.", "Dead Zone config"},
  {"Mode", "Quadrotor Channel", "nRF24 Channel"},
  {"TFT Theme", "TFT Rotation", "MCU Voltage"},
  {"Gyroscope OFF", "Gyroscope ON"}
};
#else
char *menu_str_b[3][3] = {
  {"Joystick Correct.", "Dead Zone config"},
  {"Mode", "Quadrotor Channel", "nRF24 Channel"},
  {"TFT Theme", "TFT Rotation", "MCU Voltage"},
};
#endif

void TFT_menu(int8_t _num, char *_data)
{
  tft.drawRect(7, 49 + 15 * _num, 114, 16, tft_colorA);
  tft.setCursor(10, 54 + 15 * _num);
  tft.print(_data);
}

void TFT_menu(int8_t _num, int16_t _data)
{
  tft.drawRect(7, 49 + 15 * _num, 114, 16, tft_colorA);
  tft.setCursor(10, 54 + 15 * _num);
  tft.print(_data);
}

void TFT_cursor(int8_t _num)
{
  tft.drawLine(1, 51 + 15 * _num, 4, 56 + 15 * _num, tft_colorA);
  tft.drawLine(4, 57 + 15 * _num, 1, 62 + 15 * _num, tft_colorA);
  tft.drawLine(1, 51 + 15 * _num, 1, 62 + 15 * _num, tft_colorA);
}

boolean return_menu = false;

boolean TFT_config()
{
  tft.setTextColor( tft_colorA);

  if (key_get(0, 1)) {
    menu_sta --;
    tft_cache = 1;

    if (menu_sta <= 0)
      menu_num_B = 0; //zero
  }
  if (key_get(1, 1)) {
    if (return_menu)
      menu_sta --;
    else
      menu_sta ++;
    tft_cache = 1;
  }

  if (menu_sta > 2)
    menu_sta = 2;
  if (menu_sta < 0)
    menu_sta = 0;

  return_menu = false;
  //-------------------------------
  if (tft_cache)
    tft.fillRect(0, 40, tft_width, 100, tft_colorB);

  if (menu_sta == 2) {
    switch (menu_num_A) {
      case 0: {
          switch (menu_num_B) {
            case 0: {
                if (tft_cache)
                {
                  for (uint8_t a = 0; a < 4; a++)
                  {
                    joy_correct_min[a] = 0;
                    joy_correct_max[a] = 0;
                  }
                }
                for (uint8_t a = 0; a < 4; a++) {
                  tft.setCursor(2, 120);
                  tft.print("Move Joystick MaxGear");
                  int16_t _c = Joy_i(a, false, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
                  if (_c > joy_correct_max[a]) {
                    tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                    joy_correct_max[a] = _c;
                  }
                  //                  joy_correct_max[a] = constrain(joy_correct_max[a], 0, Joy_maximum);
                  if (_c < joy_correct_min[a]) {
                    tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                    joy_correct_min[a] = _c;
                  }
                  //                  joy_correct_min[a] = constrain(joy_correct_min[a], -Joy_maximum, 0);
                }

                for (uint8_t d = 0; d < 2; d++) {
                  tft.drawFastHLine(12 + 70 * d, 80, 33, tft_colorA);
                  tft.drawFastVLine(28 + 70 * d, 64, 33, tft_colorA);
                  //                tft.fillRect(2, 90-4, 20, 12, tft_colorB);
                  tft.drawCircle(44 + 70 * d, 80, map(joy_correct_min[0 + 2 * d], 0, -512, 1, 10), tft_colorA);
                  tft.drawCircle(12 + 70 * d, 80, map(joy_correct_max[0 + 2 * d], 0, 512, 1, 10), tft_colorA);
                  tft.drawCircle(28 + 70 * d, 64, map(joy_correct_min[1 + 2 * d], 0, -512, 1, 10), tft_colorA);
                  tft.drawCircle(28 + 70 * d, 96, map(joy_correct_max[1 + 2 * d], 0, 512, 1, 10), tft_colorA);
                }
                return_menu = true;
              }
              break;
            case 1: {
                if (key_get(2, 1)) {
                  Joy_deadzone_val--;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }
                if (key_get(3, 1)) {
                  Joy_deadzone_val++;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }
                Joy_deadzone_val = constrain(Joy_deadzone_val, 0, 25);

                TFT_menu(0, Joy_deadzone_val);
                TFT_cursor(0);
                return_menu = true;
              }
              break;
          }
        }
        break;

      case 1: {
          switch (menu_num_B) {
            case 0: {
                char *menu_str_c[2] = { "Quadro.", "nRF24"};
                if (key_get(2, 1) || key_get(3, 1)) {
                  mode_protocol = !mode_protocol;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }
                for (uint8_t c = 0; c < 2; c++) {
                  TFT_menu(c, menu_str_c[c]);
                }

                TFT_cursor(mode_protocol);
                return_menu = true;
              }
              break;
            case 1: {
#if !defined(__AVR_ATmega128RFA1__)
                char *menu_str_c[5] = {"9600", "19200", "38400", "57600", "115200"};
#endif
                if (key_get(2, 1)) {
                  mwc_channal--;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }
                if (key_get(3, 1)) {
                  mwc_channal++;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }

#if !defined(__AVR_ATmega128RFA1__)
                mwc_channal = constrain(mwc_channal, 0, 4);
                TFT_menu(0, menu_str_c[mwc_channal]);
#else
                mwc_channal = constrain(mwc_channal, 11, 26);
                TFT_menu(0, mwc_channal);
#endif
                TFT_cursor(0);
                return_menu = true;
              }
              break;

            case 2: {
                if (key_get(2, 1)) {
                  nrf_channal--;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }
                if (key_get(3, 1)) {
                  nrf_channal++;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }
                nrf_channal = constrain(nrf_channal, 0, 125);

                TFT_menu(0, nrf_channal);
                TFT_cursor(0);
                return_menu = true;
              }
              break;
          }
        }
        break;
      case 2: {
          switch (menu_num_B) {
            case 0: {
                tft_theme = !tft_theme;
                TFT_init(true, tft_rotation);
                tft_cache = 1;
                tft.setTextColor(tft_colorA);
                menu_sta --;
              }
              break;
            case 1: {
                tft_rotation = !tft_rotation;
                TFT_init(true, tft_rotation);
                tft_cache = 1;
                tft.setTextColor(tft_colorA);
                menu_sta --;
              }
              break;
            case 2: {
                char *menu_str_c[2] = { "3.3V", "5.0V"};
                return_menu = true;

                if (key_get(2, 1) || key_get(3, 1)) {
                  mcu_voltage = !mcu_voltage;
                  tft.fillRect(0, 40, tft_width, 100, tft_colorB);
                }

                TFT_cursor(mcu_voltage);

                for (uint8_t c = 0; c < 2; c++) {
                  TFT_menu(c, menu_str_c[c]);
                }
                //                tft.fillRect(0, 40, tft_width, 100,tft_colorB);
              }
              break;
          }

        }
        break;

#if !(defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__))
      case 3: { //mpu
          mode_mpu = menu_num_B;
          tft_cache = 1;
          menu_sta = 0; //back main menu
          menu_num_B = 0; //zero
        }
        break;
#endif
    }
  }

  /*
    Serial.print(menu_sta);
    Serial.print(",");
    Serial.print(menu_num_A);
    Serial.print(",");
    Serial.println(menu_num_B);
  */
  //----------------------------
  if (menu_sta == 1) {
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
    int8_t meun_b_max[5] = {1, 2, 2, 1, 0};
#else
    int8_t meun_b_max[4] = {1, 2, 2, 0};
#endif
    if (!meun_b_max[menu_num_A])
      return false;
    else {
      if (key_get(2, 1)) {
        tft.fillRect(0, 40, 5, 100, tft_colorB);
        menu_num_B--;
      }
      if (key_get(3, 1)) {
        tft.fillRect(0, 40, 5, 100, tft_colorB);
        menu_num_B++;
      }
      menu_num_B = constrain(menu_num_B, 0, meun_b_max[menu_num_A]);

      TFT_cursor(menu_num_B);

      if (tft_cache) {
        for (uint8_t b = 0; b < (meun_b_max[menu_num_A] + 1); b++) {
          TFT_menu(b, menu_str_b[menu_num_A][b]);
        }
      }
    }
  }

  //main menu
  if (menu_sta == 0) {
    //custer
    if (key_get(2, 1)) {
      tft.fillRect(0, 40, 5, 100, tft_colorB);
      menu_num_A--;
    }
    if (key_get(3, 1)) {
      tft.fillRect(0, 40, 5, 100, tft_colorB);
      menu_num_A++;
    }
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
    menu_num_A = constrain(menu_num_A, 0, 4);
#else
    menu_num_A = constrain(menu_num_A, 0, 3);
#endif

    TFT_cursor(menu_num_A);

    if (tft_cache) {
#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__)
      for (uint8_t a = 0; a < 5; a++) {
#else
      for (uint8_t a = 0; a < 4; a++) {
#endif
        TFT_menu(a, menu_str_a[a]);
      }
    }
  }

  if (tft_cache) {
    //BACK
    tft.fillCircle(12, 149, 8, tft_colorA);
    tft.drawLine(11, 145, 7, 149, tft_colorB);
    tft.drawLine(7, 149, 11, 153, tft_colorB);
    tft.drawLine(7, 149, 17, 149, tft_colorB);
    //ENTER
    tft.fillCircle(12 + 20, 149, 8, tft_colorA);
    tft.drawLine(10 + 20, 146, 7 + 20, 149, tft_colorB);
    tft.drawLine(7 + 20, 149, 10 + 20, 152, tft_colorB);
    tft.drawLine(7 + 20, 149, 15 + 20, 149, tft_colorB);
    tft.drawLine(15 + 20, 146, 15 + 20, 149, tft_colorB);
    //PREV
    tft.fillCircle(127 - 12, 149, 8, tft_colorA);
    tft.drawLine(127 - 12, 153, 127 - 8, 149, tft_colorB);
    tft.drawLine(127 - 12, 153, 127 - 16, 149, tft_colorB);
    tft.drawLine(127 - 12, 153, 127 - 12, 145, tft_colorB);
    //NEXT
    tft.fillCircle(127 - 32, 149, 8, tft_colorA);
    tft.drawLine(127 - 32, 145, 127 - 28, 149, tft_colorB);
    tft.drawLine(127 - 32, 145, 127 - 36, 149, tft_colorB);
    tft.drawLine(127 - 32, 145, 127 - 32, 153, tft_colorB);
  }
  tft_cache --;
  if (tft_cache < 0)  tft_cache = 0;

  return true;
}

//------------------
#define _C_x_S  (_Q_x + 1)
#define _C_x_M  (_Q_x + ((_W_x + 1) / 2))
#define _C_x_E  (_Q_x + _W_x - 1)

char *NAME[8] = {
  "ROLL", "PITCH", "YAW", "THROT", "AUX1", "AUX2", "AUX3", "AUX4"
};

void TFT_ready()
{
  tft.fillRect(0, 0, 128, 26, tft_colorA);

  tft.drawRect(tft_width - tft_bat_x - tft_bat_x_s - 2, 2, tft_bat_x, tft_bat_y, tft_colorB);
  tft.drawRect(tft_width - tft_bat_x_s - 2, 2 + (tft_bat_y - tft_bat_y_s) / 2, tft_bat_x_s, tft_bat_y_s, tft_colorB);

  tft.setTextColor(tft_colorB);
  setFont_S;

  tft.setCursor(_Q_font_x, 3);
  tft.print(mode_protocol ? "nRF24" : "Quadr");
  tft.print(" CHAN.");
  tft.print(mode_protocol ? nrf_channal : mwc_channal);
  tft.setCursor(_Q_font_x, 16);
  tft.print("Time:");

  tft.setTextColor(tft_colorA);
  for (uint8_t a = 0; a < 8; a++) {
    tft.setCursor(_Q_font_x, _Q_font_y + a * 15);
    tft.print(NAME[a]);
    //------------------------------------------
    tft.drawRect(_Q_x, _Q_y + a * 15, _W_x, _W_y, tft_colorA);
  }
}

boolean _a = false, _b = false;
void TFT_run()
{
  if (outBuf[3] > (Joy_MID - Joy_maximum)) {
    if (_a) {
      Joy_time[0] = millis() - Joy_time[1];
      _a = false;
    }
    Joy_time[1] = millis() - Joy_time[0];
  }
  else
    _a = true;

  if (!_b && ((Joy_time[1] / 1000) % 2)) {
    _b = !_b;
    tft.fillRect(_Q_font_x + 30, 16, 50, 7, tft_colorA);
    tft.setTextColor(tft_colorB);
    tft.setCursor(_Q_font_x + 30, 16);
    tft.print((Joy_time[1] / 1000) / 60);
    tft.print("m");
    tft.print((Joy_time[1] / 1000) % 60);
    tft.print("s");
  }
  _b = boolean((Joy_time[1] / 1000) % 2);

  //battery------------------
  tft.fillRect(tft_width - tft_bat_x - 3, 3, map(_V_bat, _V_min, _V_max, 0, tft_bat_x - 2) , tft_bat_y - 2, tft_colorB);
  tft.fillRect(tft_width - tft_bat_x - 3 + map(_V_bat, _V_min, _V_max, 0, tft_bat_x - 2), 3, map(_V_bat, _V_min, _V_max, tft_bat_x - 2, 0) , tft_bat_y - 2, tft_colorA);

  for (uint8_t a = 0; a < 8; a++) {
    int8_t _C_x_A0, _C_x_B0, _C_x_A, _C_x_B, _C_x_A1, _C_x_B1;
    int8_t _C_x;

    if (outBuf[a] < Joy_MID) {
      _C_x = map(outBuf[a], Joy_MID - Joy_maximum, Joy_MID, _C_x_S, _C_x_M);

      _C_x_A0 = _C_x_S;
      _C_x_B0 = _C_x - _C_x_S;

      _C_x_A = _C_x;
      _C_x_B = _C_x_M - _C_x;

      _C_x_A1 = _C_x_M;
      _C_x_B1 = _C_x_E - _C_x_M;
    } else if (outBuf[a] > Joy_MID) {
      _C_x = map(outBuf[a], Joy_MID, Joy_MID + Joy_maximum, _C_x_M, _C_x_E);

      _C_x_A0 = _C_x_S;
      _C_x_B0 = _C_x_M - _C_x_S;

      _C_x_A = _C_x_M;
      _C_x_B = _C_x - _C_x_M;

      _C_x_A1 = _C_x;
      _C_x_B1 = _C_x_E - _C_x;
    } else {
      _C_x_A0 = _C_x_S;
      _C_x_B0 = _C_x_M - _C_x_S;

      _C_x_A = _C_x_M;
      _C_x_B = 0;

      _C_x_A1 = _C_x_M;
      _C_x_B1 = _C_x_E - _C_x_M;
    }
    tft.fillRect(_C_x_A0,  _Q_y + a * 15 + 1, _C_x_B0, _W_y - 2, tft_colorB);
    tft.fillRect(_C_x_A,  _Q_y + a * 15 + 1, _C_x_B, _W_y - 2, tft_colorC);
    tft.fillRect(_C_x_A1,  _Q_y + a * 15 + 1, _C_x_B1, _W_y - 2, tft_colorB);

    tft.fillRect(_C_x_M,  _Q_y + a * 15 - 1, 1, _W_y + 2, tft_colorD);
  }
  //netsta------------------
  tft.fillRect(0, 158, 128, 2, node_clock_error ? tft_colorD : tft_colorC);
}

time.h 

#include "Arduino.h"

//unsigned long time;
unsigned long TIME1;            //setup delay
unsigned long time2; //send data
unsigned long time3; //battery
unsigned long Joy_time[2] = {0, 0}; //joy

视频
取自“http:///https//wiki.microduino.cn/index.php?title=自平衡小车/zh&oldid=11436”