Open Source Self-balance Robot System

From Microduino Wiki
Revision as of 09:59, 30 December 2015 by Admin (talk) (Created page with "{| style="width: 800px;" |- | ==Outline== *Project: Microduino Self-balance Car *Objective: To make a self-balance car that can be controlled with a remote controller. *Diff...")
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

Outline

  • Project: Microduino Self-balance Car
  • Objective: To make a self-balance car that can be controlled with a remote controller.
  • Difficulty: Middle
  • Time-consuming: 2 Hours
  • Maker:
  • Introduction:

This two-wheel self-balance car is an integrated system with multiple functions, which is also a study subject focusing on automatic control theory, dynamics theory and technology theory. The essential problem is to solve self-balance and to complete tasks under various kinds of environment. In this tutorial, we adopt Microduino modules to build a self-balance car controlled by the Joypad remote controller. It enables users to play it in short time.

Bill of Materials

  • Microduino Equipment
Module Number Function
Microduino-Core+ 1 Core board
Microduino-USBTTL 1 Program download
Microduino-nRF24 1 Wireless communication
Microduino-Robot 1 Driving and connection board
Microduino-Stepper 2 Driving stepper motor
  • Other Equipment
Module Number Function
2.4G Antenna 1 2.4G communication
Frame 1 For fixating
Nylon screw 4 For fixating
Nylon nut 12 For fixating
Battery box 1 Battery loader
Battery 2 Power supply
Micro-USB cable 1 Serial communication and program download
Axis 2 Wheel connection
Wheel 2 Structure
Stepper motor 2 Wheel driving
  • Joypad Material
Module Number Function
Microduino-Core 1 Core module
Microduino-nRF24 1 Wireless communication
Microduino-Joypad 1 Remote controller
Microduino-TFT 1 Display
Microduino-USBTTL 1 Program download
Lithium battery 1 Power supply
Acrylic panel 1 Front panel
Nylon column 4 For fixating
Long nylon screw 4 For fixating
Short nylon screw 4 For fixating
Nylon nut 4 For fixating
Sponge panel 1 For fixating
平衡车物料.jpg

Principle of the Experiment

  • PID Principle

PID makes difference in the automatic control area. Let's first get to know what is PID. PID refers to Proportional, Integral and Derivative. If we want the controlled object to be a steady result, we can use PID algorithm. Assuming that there is a car moves with a speed of 1m/s and we want to change that to 5m/s, we must have:

  • A car driving device.
  • A device that can detect the current speed.

We can force the car to move until it gets to 5m/s and then withdraw the force. However, that will bring about problems as follows: 1)When the speed of the car reaches 5m/s, the device will detect the speed and notice the controller to make it change the output voltage. In that process, it takes time and in this time, the car may be accelerated more or less. 2)After withdrawing the force, outside conditions such as fraction will affect the speed of the car. However, PID algorithm can solve those problems in a certain error range. Here is how PID works. In every sampling cycle, the speed can be detected by a certain device and then transmitted into the program, and get a new speed through the program calculation. In the next sampling cycle, it'll get the new speed transmitted, then acquire a new voltage and so forth. The key of PID is how to get the car steady based on the current speed value and voltage. PID algorithm adopts proportion, integral and differential to control. Three methods correspond to a constant quantity, that is pconst, iconst and dconst respectively. These three constants are acquired under times of experiment. We can use a mathematical formula to express PID as follows: e=Expected value - input value.

PIDtheory.jpg

The reason why a self-balance car can keep balance is the Microduino-10DOF module with an accelerator and a gyroscope to detect the corresponding altitude data, then use Kalman algorithm to get the current of the car. The angle of the balance is 180 degrees. If the measured balance deviates from 180 degrees, the PID algorithm will adjust and output the relevant PWM values to the motor to keep the balance of the car. The main code of PID:

// PD control.  
float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd) 
{ 
    float error; 
    float output; 
    error = setPoint - input; 
    // Implementation of Kd in two parts 
    //Only used part of the input rather than the biggest setting value of the input(T-2).
    //The second one adopts the setting value.
    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;  // The error is the only input component of the Kd value. 
    setPointOld = setPoint; 
    return(output); 
} 
//Realize "P" control. 
float speedPControl(float input, float setPoint,  float Kp) 
{ 
    float error; 
    error = setPoint - input; 
    return(Kp * error); 
}

The kit makes use of a gyroscope and an acceleration sensor (Microduino-10DOF) to detect the altitude change of the car, and adopts Microduino-Stepper to adjust the motor precisely and keep balance.

PIDtheory1.jpg
  • The Main Sensor

Microduino-10DOF

Document

Program Download

Stack Microduino-Core+ and Microduino-USBTTL together, then connect them to a computer with a USB cable.

Download1.jpg

Make sure you build Microduino IDE. If not, please refer to 1-Arduino IDE installation instruction.

Gettingstarted.jpg

Open Arduino IDE, click【File】->【Open】

Dl1.jpg

Scan the project program address, click " Joypad_Balance_Reception.ino " and open.

Balancecaropen1.jpg
Balancecaropen2.jpg

Click "Tools", select Microduino-Core+ as the board and the Atmega644pa@16M,5V as the processor. Then choose the right port and download program directly.

WiFiStationopen4.jpg

Self-balance Car Buildup

  • Step1:First, connect A1 and A2, then plug the B1 and B2 into the two sides of A1.
Balancestep1.jpg
  • Step2:Connect the motor with C1 and then fixate them with screws.
Balancestep2.jpg
  • Step3:Connect the parts built in Step 1 and Step 2, and then fixate them with B1.
Balancestep3.jpg
  • Step4:Fixate the wheels onto the motor with screws and nuts.
Balancestep4.jpg
  • Step5:Put the battery module on the specific position as the picture shows, use C1 and C3 to connect to the corresponding positions, then fixate them with C4, A3 and C5.
Balancestep5.jpg
  • Step6:Connect the Core+, USBTTL, 10DOF and the base plate, fixate them with screws and nuts, use sensor cables and batteries to connect to the place as the picture shows:
Balancestep6.jpg

Joypad Buildup

  • Step 1:Insert Microduino-TFT into the Microduino-Joypad panel and fixate them with screws. Please mind the installation order of Microduino-TFT.
Joypadstep1.jpg
  • Step 2:Insert the sensor cable into the interface of Microduino-TFT.
Joypadstep2.jpg
  • Step 3:Put the two Joystick keys, batteries and four white keys in the right place, then install the nRF24 module and Core module on the base plate of the Joypad.
Joypadstep3.jpg
  • Step 4:Connect the other side of the Microduino-TFT cable to the right place of the base plate, then put the long screws into the four s of the panel.
Joypadstep4.jpg
  • Step 5:Fixate the shell and the base plate of the Joypad with screws and nuts.
Joypadstep5.jpg
  • Step 6:Tear off the sticker of the antenna after installation and attach it to the base plate of the Joypad. Congratulations! You just completed the Joypad installation.
Joypadstep6.jpg
  • Self-balance Car and Joypad Debugging

Joypad Debugging

  • Key Setting

Press down Key1 within 4s after the open of the Joypad and enter Config mode.

Step1进入设置.jpg
  • Enter Setting mode

Key1- Key4 from the left to right. Please refer to the color of the picture.

Step1按键对应.jpg

Note: Please enter the setting mode before the OS interface. If not, please restart and enter.

  • Joystick Calibration

Press down Key3 and Key4 to move the cursor. The Key1 means "Return" and the Key2 means "Confirm". Select Joystick Config and enter the Joystick setting mode. Choose Joystick Correct to enter the calibration mode. A After that, it'll appear the interface shown in the third picture with an initializing state of two crosses. At the this time, you can move the joystick around. Then, you can see circles in four directions and biggest circle means the Joystick has been turned to the limit position. Press Key2 to confirm after the calibration and return to the last page.

Step2摇杆校准.jpg
  • Select the control mode

Press Key1 to return to the main page and select Protocol Config to enter mode selection. Select the first Mode, then choose nRF24(namely, Robot control mode), press Key2 to confirm and return.

Step3设置Robot模式.jpg
  • Set communication channel

Return to the secondary list, select nRF24 Channel and press Key2 to confirm. Select 70, which corresponds to the function setting of the nRF24 in Robot_Microduino.ino.

Step4通信通道设置robot.jpg

Attention

  • Please make sure you only stack the Core (Core+) and the USBTTL modules when downloading the program. Although the nRF24 will not cause conflict, other communication modules may lead to serial conflict.
  • Make sure the right cathode connection of the lithium battery, or it'll burn the circuit.
  • After the debugging, please don't adopt USB power supply when running the program and use battery instead, or it may cause undervoltage.

Program Description

Address of the baidu cloud disk: http://pan.baidu.com/s/1eQBaMjg Code:h08a

Github address: https://github.com/wasdpkj/BalanceCar_Microduino/tree/master/BalanceCar_Microduino

#define MOTOR_EN  4      //PORTB,0
#define MOTOR1_DIR A0    //PORTA,7
#define MOTOR1_STEP 5   //PORTB,1
#define MOTOR2_DIR A1    //PORTA,6
#define MOTOR2_STEP 6   //PORTB,2
#define MOTOR3_DIR A2    //PORTA,5
#define MOTOR3_STEP 7   //PORTB,3
#define MOTOR4_DIR A3    //PORTA,4
#define MOTOR4_STEP 8   //PORTD,6

#define LED -1
//rf=======================================
#include <RF24Network.h>
#include <RF24.h>
#include <SPI.h>

// nRF24L01(+) radio attached using Getting Started board
RF24 radio(9, 10);
RF24Network network(radio);
const uint16_t this_node = 1;	//Set ID
const uint16_t other_node = 0;

//--------------------------------
struct send_a	//Send 
{
  uint32_t node_ms;		//Node running time 
};

unsigned long last_sent = 0;	//Timer 

struct receive_a  //Receive 
{
  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;
};

unsigned long clock;	//Host machine running time 
int tem_xuan = 0;			//Host machine request schedule 

//----------------------------
boolean node_STA = false;

unsigned long safe_ms = millis();
//=================================================================================
#include <Wire.h>
#include <I2Cdev.h>
#include <JJ_MPU6050_DMP_6Axis.h>  // DMP working library's revised version. 

//=================================================================================
#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))

//=========================================
#define DEBUG 0

#define ZERO_SPEED 65535 //Zero speed 
#define MAX_ACCEL 7 //The maximum ACCEL
#define MAX_THROTTLE 480 //The maximum throttle is 530. 
#define MAX_STEERING 120 //The maximum steering
#define MAX_TARGET_ANGLE 15 //The maximum target  is 12.  

// Default control items  
#define ANGLE_FIX -3.5
#define KP 0.22 // 0.22        
#define KD 28  // 30 28 26  
#define KP_THROTTLE 0.065  //0.08//0.065
#define KI_THROTTLE 0.05//0.05
// For "raiseup" control gains.  
#define KP_RAISEUP 0.02 //kp raise up  
#define KD_RAISEUP 40   //kd raise up 
#define KP_THROTTLE_RAISEUP 0  //No speed control on "raiseup"
#define KI_THROTTLE_RAISEUP 0.0//Throttle raise up 
#define ITERM_MAX_ERROR 40   // ITERM saturation constant 
#define ITERM_MAX 5000
//=================================================================================

#define I2C_SPEED 400000L //I2C speed 
#define Gyro_Gain 0.03048//Gyroscope gain 
#define Gyro_Scaled(x) x*Gyro_Gain //Return the original data of the gyroscope 
#define RAD2GRAD 57.2957795//57.2957795
#define GRAD2RAD 0.01745329251994329576923690768489//0.01745329251994329576923690768489

//=================================================================================
bool Robot_shutdown = false; // Robot shutdown flag => Out of battery
uint8_t mode;   // 0: MANUAL MODE   1: autonomous MODE
uint8_t autonomous_mode_status;   // 1: NORMAL STATUS=Walking  2:
int16_t autonomous_mode_counter;
int16_t autonomous_mode_distance;
int16_t pushUp_counter;  // for pushUp functionality (experimental)

// MPU Control/Status 
bool dmpReady = false;  // Set as True if DMP initialization is ready. 
uint8_t mpuIntStatus;   // Own MPU interruption status bytes 
uint8_t devStatus;      // Each deveice will return to the original status after operation. (0=Ready! 0=False)
uint16_t packetSize;    // Predict DMP packet(Here is 18 bytes.
uint16_t fifoCount;     // All bytes in FIFO. 
uint8_t fifoBuffer[18]; // FIFO buffer 
Quaternion q;
uint8_t loop_counter;       // Generate 40Hz loop 
uint8_t slow_loop_counter;  // Slow loop 2HZ
long timer_old;//Old timer 
long timer_value;//Timer value 
int debug_counter;//Debug the counter 
float dt;
int lkf;
// The default I2C daddress is 0X68
MPU6050 mpu;
float angle_adjusted;//Angle adjustment 
float angle_adjusted_Old;// Angle adjustment 
float Kp = KP;
float Kd = KD;
float Kp_thr = KP_THROTTLE; //Throttle 
float Ki_thr = KI_THROTTLE;
float Kp_user = KP;
float Kd_user = KD;
float Kp_thr_user = KP_THROTTLE;
float Ki_thr_user = KI_THROTTLE;
bool newControlParameters = false;//New Bool control parameters 
bool modifing_control_parameters = false; // New Bool control parameters 
float PID_errorSum;
float PID_errorOld = 0;
float PID_errorOld2 = 0;
float setPointOld = 0;
float target_angle;
float throttle;
float kkll;
float steering;
float max_throttle = MAX_THROTTLE;//
float max_steering = MAX_STEERING;
float max_target_angle = MAX_TARGET_ANGLE;
float control_output;
int16_t motor1;
int16_t motor2;
int16_t speed_m[2];           // Actual motor rotation speed
uint8_t dir_m[2];             // Actual rotation direction of the stepper motor 
int16_t actual_robot_speed;          // Actual robot speed 
int16_t actual_robot_speed_Old;          // Actual robot speed
float estimated_speed_filtered;//Filtered estimated speed 
uint16_t counter_m[2];        // Cycle of the counter 
uint16_t period_m[2][8];      //  
uint8_t period_m_index[2];    // 
int freeRam () { //Free Ram 
  extern int __heap_start, *__brkval;
  int v;
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}
//DMP Function 
//This function defines the weight on the accelerated sensor. 
//0x80 by default 
//The official name of InvenSense is inv_key_0_96(?)
void dmpSetSensorFusionAccelGain(uint8_t gain) {
  // INV_KEY_0_96
  mpu.setMemoryBank(0);
  mpu.setMemoryStartAddress(0x60);
  mpu.writeMemoryByte(0);
  mpu.writeMemoryByte(gain);
  mpu.writeMemoryByte(0);
  mpu.writeMemoryByte(0);
}
// Calculate and get Obtein angle
float dmpGetPhi() {
  mpu.getFIFOBytes(fifoBuffer, 16); // Get FIFO bytes 
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.resetFIFO();  //Reset FIFO
  //Return ( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
  return (atan2(2 * (q.z * q.y + q.x * q.w), q.x * q.x - q.w * q.w - q.z * q.z + q.y * q.y) * RAD2GRAD);
}
//xyzw
//wzyx

// PD's implementation. DT is "ms". 
float stabilityPDControl(float DT, float input, float setPoint,  float Kp, float Kd)
{
  float error;
  float output;
  error = setPoint - input;
  // Implementation of Kd's two parts 
  //Only adopt parts of the sensor input ranther than the maximum set value (T-2). 
  //However, the second adopts that set value. 
  output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; // + Erro - PID_error_Old2
  //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
  PID_errorOld2 = PID_errorOld;
  PID_errorOld = input;  // Error is the only input component of Kd.  
  setPointOld = setPoint;
  return (output);
}
//P control realizes. 
float speedPControl(float input, float setPoint,  float Kp)
{
  float error;
  error = setPoint - input;
  return (Kp * error);
}
// PI realizes; DT is "ms". 
float speedPIControl(float DT, float input, float setPoint,  float Kp, float Ki)
{
  float error;
  float output;
  error = setPoint - input;
  PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
  PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);
  output = Kp * error + Ki * PID_errorSum * DT * 0.001;
  return (output);
}

void calculateSubperiods(uint8_t motor)
{
  int subperiod;
  int absSpeed;
  uint8_t j;

  if (speed_m[motor] == 0)
  {
    for (j = 0; j < 8; j++)
      period_m[motor][j] = ZERO_SPEED;
    return;
  }
  if (speed_m[motor] > 0 )   // Positive speed 
  {
    dir_m[motor] = 1;
    absSpeed = speed_m[motor];
  }
  else                       // Negative speed 
  {
    dir_m[motor] = 0;
    absSpeed = -speed_m[motor];
  }

  for (j = 0; j < 8; j++)
    period_m[motor][j] = 1000 / absSpeed;
  // Calculate subage. If module <0.25=>sub-period= 0. If module<0.75 sub-period=2


 If module <0.5=>sub-period= 1. If module <0.75 sub-period=2 other sub-periods =3
  subperiod = ((1000 % absSpeed) * 8) / absSpeed; // Optimize code to calculate the sub-period. (Integer arithmetic)
  if (subperiod > 0)
    period_m[motor][1]++;
  if (subperiod > 1)
    period_m[motor][5]++;
  if (subperiod > 2)
    period_m[motor][3]++;
  if (subperiod > 3)
    period_m[motor][7]++;
  if (subperiod > 4)
    period_m[motor][0]++;
  if (subperiod > 5)
    period_m[motor][4]++;
  if (subperiod > 6)
    period_m[motor][2]++;
}
void setMotorSpeed(uint8_t motor, int16_t tspeed)
{
  // We limite the maximum speed 
  if ((speed_m[motor] - tspeed) > MAX_ACCEL)
    speed_m[motor] -= MAX_ACCEL;
  else if ((speed_m[motor] - tspeed) < -MAX_ACCEL)
    speed_m[motor] += MAX_ACCEL;
  else
    speed_m[motor] = tspeed;
  calculateSubperiods(motor);  //We adopt four sub-periods to enhance resolution.
  // Save energy and don't run it. 
  if ((speed_m[0] == 0) && (speed_m[1] == 0))
  {
    digitalWrite(MOTOR_EN, HIGH);  //Don't use the motor 
  }
  else
  {
    digitalWrite(MOTOR_EN, LOW);  // Use the motor 
  }
}

void setup()
{
  Serial.begin(115200);

  // Stepper pins 
  pinMode(MOTOR_EN, OUTPUT);   // ENABLE MOTORS

  pinMode(MOTOR1_DIR, OUTPUT);  // DIR MOTOR 1
  pinMode(MOTOR1_STEP, OUTPUT);  // STEP MOTOR 1

  pinMode(MOTOR4_DIR, OUTPUT); // DIR MOTOR 4
  pinMode(MOTOR4_STEP, OUTPUT); // STEP MOTOR 4

  digitalWrite(MOTOR_EN, HIGH);  // Disbale motors

  pinMode(LED, OUTPUT);   // ENABLE MOTORS

  //mpu==============================
  // Add in I2C bus 
  Wire.begin();
  // 4000Khz fast mode
  TWSR = 0;
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
  TWCR = 1 << TWEN;

  digitalWrite(LED, HIGH);
  delay(200);
  digitalWrite(LED, LOW);
  delay(200);

  Serial.println("Initializing I2C devices...");
  //mpu.initialize();
  mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setDLPFMode(MPU6050_DLPF_BW_20);  //10,20,42,98,188
  mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz
  mpu.setSleepEnabled(false);

  digitalWrite(LED, HIGH);
  delay(500);
  digitalWrite(LED, LOW);
  delay(500);

  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0)
  {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;

    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(200);
  }
  else
  { // ERROR!
    while (1)
    {
      // 1 = initial memory load failed
      // 2 = DMP configuration updates failed
      // (if it's going to break, usually the code will be 1)
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));

      digitalWrite(LED, HIGH);
      delay(1000);
      digitalWrite(LED, LOW);
      delay(200);
    }
  }

  // Calibration of the gyroscope 
  //The robotmust be steady when initializing. 
  // It needs to reset and calibrate the gyroscope. 
  digitalWrite(LED, HIGH);
  delay(200);
  digitalWrite(LED, LOW);
  delay(500);
  digitalWrite(LED, HIGH);
  delay(200);
  digitalWrite(LED, LOW);

  Serial.print("Free RAM: ");
  Serial.println(freeRam());
  Serial.print("Max_throttle: ");
  Serial.println(max_throttle);
  Serial.print("Max_steering: ");
  Serial.println(max_steering);
  Serial.print("Max_target_angle: ");
  Serial.println(max_target_angle);

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  timer_old = millis();

  TIMER_INT();

  //Adjust DMP sensor fusion 
  Serial.println("Adjusting DMP sensor fusion gain...");
  dmpSetSensorFusionAccelGain(0x40);//0x20///////////////////////////////Adjust the sensor
  Serial.println("Initializing Stepper motors...");

  digitalWrite(MOTOR_EN, LOW);   // Stepper driver  

  // The vibration of the motor indicates the robot is ready. 

  for (uint8_t k = 0; k < 3; k++)
  {
    setMotorSpeed(0, 3);
    setMotorSpeed(1, -3);
    delay(150);
    setMotorSpeed(0, -3);
    setMotorSpeed(1, 3);
    delay(150);
  }

  mpu.resetFIFO();
  timer_old = millis();

  //nRF==============================
  SPI.begin();		//Initialize SPI bus 
  radio.begin();
  network.begin(/*channel*/ 70, /*node address*/ this_node);

  digitalWrite(LED, HIGH);
  Serial.println("===========start===========");
}
// Major loop//////////////////////////////////////////////////////////////////////////
void loop()
{
  //===============================================================
  if (nRF(&throttle, &steering))
  {
    /*
    Serial.print(Speed_need);
    Serial.print(",");
    Serial.println(Turn_need);
    */
  }

  //===============================================================
  if (safe_ms > millis()) safe_ms = millis();
  if (millis() - safe_ms > 1000)
  {
    steering = 0;
    throttle = 0;
  }

  //===============================================================
  robot();
}

void robot()
{
  //===============================================================
  timer_value = millis();

  // New DMP positioning solution
  fifoCount = mpu.getFIFOCount();
  if (fifoCount >= 18)
  {
    if (fifoCount > 18) // If we have one or more data packets, we can adopt the simplest way to dump the buffer zone. 
    {
      mpu.resetFIFO();
      return;
    }

    loop_counter++;
    slow_loop_counter++;
    dt = (timer_value - timer_old);
    timer_old = timer_value;
    angle_adjusted_Old = angle_adjusted;
    angle_adjusted = dmpGetPhi() + ANGLE_FIX;
    Serial.println(angle_adjusted);
    mpu.resetFIFO();  // Rest FIFO 
    // Calculate and estimate the robot speed 
    actual_robot_speed_Old = actual_robot_speed;
    actual_robot_speed = (speed_m[1] - speed_m[0]) / 2; // Front 
    // Angle adjusted 
    int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0; 
    // Use the robot speed (T-1) or (T-2) to compensate the delay 
    int16_t estimated_speed = actual_robot_speed_Old - angular_velocity;     
    //Estimated speed 
    estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05; 
   //Target angle 
    target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
    //Limited output 
    target_angle = constrain(target_angle, -max_target_angle, max_target_angle); 

    if (pushUp_counter > 0) // pushUp mode?
      target_angle = 10;

    //Integrate output acceleration
    control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
    control_output = constrain(control_output, -800, 800); // Limite the maxiumum output control 
    // Control steering output 
    motor1 = control_output + steering;
    motor2 = -control_output + steering;   //Motor2 rotates conversely. 
    // Limit the maximum speed 
    motor1 = constrain(motor1, -500, 500);
    motor2 = constrain(motor2, -500, 500);

    // Is robot ready (upright?)
    if ((angle_adjusted < 66) && (angle_adjusted > -66))
    {
      if (node_STA) // pushUp mode?
      {
        pushUp_counter++;

        if (pushUp_counter > 60) // 0.3 seconds
        {
          // Set motors to 0 => disable steppers => robot
          setMotorSpeed(0, 0);
          setMotorSpeed(1, 0);
          // We prepare the raiseup mode
          Kp = KP_RAISEUP;   // CONTROL GAINS FOR RAISE UP
          Kd = KD_RAISEUP;
          Kp_thr = KP_THROTTLE_RAISEUP;
          control_output = 0;
          estimated_speed_filtered = 0;
        }
        else
        {
          setMotorSpeed(0, motor1);
          setMotorSpeed(1, motor2);
        }
      }
      else
      {
        // NORMAL MODE
        setMotorSpeed(0, motor1);
        setMotorSpeed(1, motor2);
        pushUp_counter = 0;
      }

      if ((angle_adjusted < 40) && (angle_adjusted > -40))
      {
        Kp = Kp_user;  // Default or user control gains
        Kd = Kd_user;
        Kp_thr = Kp_thr_user;
        Ki_thr = Ki_thr_user;
      }
      else
      {
        Kp = KP_RAISEUP;   // CONTROL GAINS FOR RAISE UP
        Kd = KD_RAISEUP;
        Kp_thr = KP_THROTTLE_RAISEUP;
        Ki_thr = KI_THROTTLE_RAISEUP;
      }
    }
    else   // Robot not ready, angle > 70º
    {
      setMotorSpeed(0, 0);
      setMotorSpeed(1, 0);
      PID_errorSum = 0;  // Reset PID I term
      Kp = KP_RAISEUP;   // CONTROL GAINS FOR RAISE UP
      Kd = KD_RAISEUP;
      Kp_thr = KP_THROTTLE_RAISEUP;
      Ki_thr = KI_THROTTLE_RAISEUP;
    }

  }
}

boolean nRF(float * _speed, float * _turn)
{
  network.update();
  // Is there anything ready for us?
  while ( network.available() )
  {
    // If so, grab it and print it out
    RF24NetworkHeader header;
    receive_a rec;
    network.read(header, &rec, sizeof(rec));

    clock = rec.ms;		//Receive the assigned running time values of the host machine.  
    float * _i = _speed;
    _i[0] = map(rec.rf_CH1, 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE);
    _i = _turn;
    _i[0] = map(rec.rf_CH0, 1000, 2000, -MAX_STEERING, MAX_STEERING);

    node_STA = (rec.rf_CH7 > 1500 ? true : false);    //Receive the assigned 


request schedule values    
    {
      //Serial.print("Sending...");
      send_a sen = {
        millis()
      };  //Send out the data corresponding to the previous sending array. 

      RF24NetworkHeader header(0);
      boolean ok = network.write(header, &sen, sizeof(sen));
      safe_ms = millis();
      if (ok)
      {
        return true;
        //Serial.println("ok.");
      }
      else
      {
        return false;
        //Serial.println("failed.");
      }
    }

    safe_ms = millis();
  }
}

Joypad Program and Description

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();		//Initialize 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();
  }

  //Node checking -------------------------------
  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  //Node checking interval. 
uint8_t nrf_channal = 70;  //0~125

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

#define _V_max 41		//The maximum voltage of the Li-ion battery is 4.2V
#define _V_min 36		// The minimum voltage of the Li-ion battery is 3.7

#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 reads assigned values. 
  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()
{
  // Define structure variable config and its content 
  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;

  // Save the variable config into EEPROM written with address 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}; //Key1,Key2, Key3 and Key4  

boolean key_status[4];			//Key 
boolean key_cache[4];		//Detect key cache when loosening it. 
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];		//Judge from the cache 

  key_status[_key_num] = !Joypad.readButton(key_pin[_key_num]);	//When triggering 

  switch (_key_type)
  {
    case 0:
      if (!key_status[_key_num] && key_cache[_key_num])		// After pressing down and loosening  
        return true;
      else
        return false;
      break;
    case 1:
      if (key_status[_key_num] && !key_cache[_key_num])		// After pressing down and loosening  
        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[])
{
  //Three parameters: Data length, instruction code and real data array. 
  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	//Set ID 
#define other_node 1

//--------------------------------
struct send_a	//Send 
{
  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	//Receive 
{
  uint32_t node_ms;
};

//--------------------------------
unsigned long node_clock, node_clock_debug, node_clock_cache = 0;		//Node running time, node response detection time and time cache. 

//debug--------------------------
boolean node_clock_error = false;	//Node response status 
unsigned long time_debug = 0;		//Timer 


//======================================
void vodebug()
{
  if (millis() - time_debug > interval_debug)
  {
    node_clock_error = boolean(node_clock == node_clock_debug);		//In a certain time, if the running time for the node returning don't change, it means there is a problem. 

    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]
  };		//Send out the data corresponding to the original sending array. 
  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;		//Assign values of the running time. 
    }
  }
#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
//-------Font setting. Big/medium/small. 
#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

Video