查看“开源平衡车/zh”的源代码

{| style="width: 800px;"
|-
|
==概述==
*项目名称：Microduino开源平衡车
*目的：DIY一台属于自己的两轮自平衡小车
*难度：中
*耗时：2小时
*制作者：
*简介:
两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本教程将指导读者基于Microduino产品模块组建一个可以遥控的自平衡小车，能够实现两轮小车平衡直立和运动的功能，用户在制作完成后，可以在该平台上进行二次开发，实现更多有趣功能。

==两轮平衡车的原理简介==
===平衡车的结构===
Microduino平衡车采用如图所示结构，车体由三层组成，从上到下依次是主控层、电池层、电机驱动层。主控层作为平衡车的大脑，集成Microduino模块和传感器；电池层作为心脏，采用2S锂电池（7.4V）给整个小车提供充足的电力；电机驱动层就是平衡车的两条腿，能够带动小车跑起来。但是以上这些构件怎么能让两轮小车能够直立不到，并且能够前后左右灵活运动呢，我们可以从以下问题分析平衡小车背后的工作原理。
===问题一：两轮平衡小车是一个怎样的结构？===
两轮车是一个高度不稳定的系统，在重力的作用下车体姿态本征不稳定，在没有外力的作用下小车必然出现倾倒的现象，其受力情况如下：
[[File:BalcanceForce.jpg||600px|center]]
在理想状态下，在小车直立的情况下，小车的重力和车轮支持力受力平衡，小车可以稳定不倒。但是在轻微的扰动下，小车偏离直立方向，小车和垂直方向形成θ角，只要产生θ角，即使角度很小，小车重心M的方向与车轮支持力H的方向亦产生了角度，合力不为0，根据牛顿运动定律可知，小车重心会向下移动，θ角度将越来越大，直至车体倾倒在地上。
===问题二：对于这样一个高度不稳定的系统，怎么能让小车保持直立不倒？===
[[File:PIDtheory.jpg||600px|center]]
从工作方式上，两轮小车的运动方式更像是人行走的过程。对于人而言，当人像前倾倒并失去平衡时，人通过自身的感觉器官能够察觉到自己身体的倾斜角度，然后人会做出反应——向前迈出一步来防止自己因失去平衡摔倒。而如果迈出一步仍不能站稳，人就会一步又一步地向前走，直到能回到平衡状态。因此对于两轮小车而言，我们可以将两个电机驱动的车轮看成是人的双腿，同时需要一个能检测小车倾斜角度的传感器，在如下的控制策略下便能够实现车体的动态平衡。
[[File:BalanceAngle.jpg||600px|center]]
图上所示，我们设定一个车体要保持的倾角，对于水平地面，我们希望车体能保持0度倾角，这个角度称为目标倾角。同时倾斜传感器会检测车体的姿态，得到一个车体的当前倾角。我们可以根据目标倾角及当前倾角的差值计算出下一步电机该怎样运动，从而能够纠正小车的姿态。小车就可以一直重复上述步骤，让小车实际倾角越来越趋近目标倾角，我们将这个循环控制称为平衡小车的角度环控制。
===问题三：在小车动态平衡的时候怎控制小车的运动速度和方向？===
在问题二中，我们可以通过倾斜传感器和电机配合能够让两轮小车保持平衡，但是电机在驱动车轮调节车体倾角的时候，小车的运动方向和速度是一直变化的。此时我们需要一个能检测车轮转速的传感器，然后利用车轮转速就能计算出小车的运动速度和方向，然后利用类似控制小车倾角的方式控制小车的速度。
[[File:BalanceSpeed.jpg||600px|center]]
图上所示，我们给小车确定一个目标速度，同时转速传感器检测车轮的转速，然后计算出小车整体的实际速度。我们就可以根据小车的目标速度和实际转速的差值调整电机转速，让小车实际速度接近目标速速。小车在一直循环上述步骤时，小车的实际速度会越来越接近目标转速，我们将小车的这个循环控制称为速度环控制。
===Microduino两轮平衡小车原理组成===
在了解了两轮小车保持平衡的几个基本问题后，两轮小车系统的基本组成原理可用如下拓扑图示意
[[File:BalanceTheory.jpg||600px|center]]
两轮平衡小车是一个典型的闭环控制系统，为了精确控制小车倾角和速度的目的，此处同时采用两个闭环控制器：角度环控制和速度环控制。
*速度环：图上左侧输入为两轮小车从遥控器接收的速度控制信号，即为目标速度，图中2处为车轮转速传感器检测并计算出的小车实际速度，速度环控制器利用这两个速度计算出一个倾角，在保持该倾角的状态下，小车即可达到实际速度等于目标速度的目的。
*角度环：图上速度环的内侧即是角度环，速度环计算出的倾角输入给角度环，即为目标角度，图中3处为小车倾角传感器检测出的小车实际倾角，角度环就是利用以上两个倾角计算出一个速度调整值，电机在调整转速后，小车即可达到实际倾角等于目标倾角的目的。
上述的两个闭环控制器一直处于动态工作状态，实时检测两轮小车的实际速度和实际倾角，并及时调整目标倾角和电机速度，这样可以实现两轮小车在不倒的同时还能按照我们想要的速度和方向运动。
===Microduino两轮小车硬件方案===
*从以上原理分析，我们可以得到两轮平衡小车的功能需求：
[[File:BalanceHardware.jpg||600px|center]]
为了实现以上功能，可以采用下列Microduino模块来满足需求：
*无线遥控：采用Microduino-Module nRF模块，2.4G无线通讯方案，能够和Microduino-Joypad配合使用。
*驱动电机：采用两个两相四线42步进电机驱动小车，步进电机是一种感应电机，可以通过控制脉冲频率来控制电机转动的转速和力矩，因此我们可以利用电机的控制频率计算出电机的当前转速。为了驱动两路步进电机，我们还需要Microduino-Shield Stepper底板，该底板能够最多驱动4路步进电机。
*转速检测：参考步进电机说明，通过电机驱动信号计算出电机的转速。
*角度检测：采用Microduino-Module Motion模块，该模块集成四种传感器，分别是三轴加速度传感器、三轴陀螺仪、磁场强度传感器和数字气压传感器。可以利用三轴加速度传感器和三轴陀螺仪*计算出模块的姿态，得到小车在纵轴方向上的倾角。
*控制核心：采用Microduino-Core+模块
*供电电源：采用2s锂电池，标准供电电压7.4V，最高可达8.4V。能够提供充沛的电力。
===Microduino两轮小车软件方案===
在确定的硬件方案下，可以通过如下软件流程实现两轮小车平衡原理。
[[File:BalanceSoftware.jpg||600px|center]]
上图中初始化部分主要包括Microduino几个主要功能模块的设置，

==材料清单==
*Microduino设备
{|class="wikitable"
|-
|模块||数量||功能
|-
|[[Microduino-Core+/zh]]||1||核心板
|-
|[[Microduino-USBTTL/zh]] ||1||下载程序
|-
|[[Microduino-Module nRF/zh]] ||1||无线通信
|-
|[[Microduino-Module Motion/zh]] ||1||检测姿态
|-
|[[Microduino-Shield Stepper/zh]] ||1||驱动连接地板

|}
*其他设备
{|class="wikitable"
|-
|模块||数量||功能
|-
|2.4G天线||1||2.4G通讯
|-
|固定板材||1||固定支撑
|-
|尼龙螺丝||4||固定
|-
|尼龙螺柱||4||固定
|-
|尼龙螺母||4||固定
|-
|2s锂电池 ||1||供电
|-
|Micro-USB数据线 ||1||串口通信，下载程序
|-
|车轴连接器 ||2||连接电机轴和车轮
|-
|车轮 ||2||结构
|-
|步进电机 ||2||驱动车轮
|-
|步进电机线 ||2||连接电机和驱动板
|}
[[File:平衡车物料.jpg||1000px|center]]

==实验原理==
*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 = 期望值 – 输入值
[[File:PIDtheory.jpg||600px|center]]
平衡车之所以可以自己掌握平衡，首先通过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的主要代码：
<source lang="cpp">
// 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); 
} 
</source>
本套件利用陀螺仪和加速度传感器（Microduino-10DOF/zh）来检测车体态的变化，并利用步进电机控制核心（Microduino-Stepper/zh），精确地驱动电机进行相应的调整，以保持系统的平衡。
[[File:PIDtheory1.jpg||600px|center]]
*主要传感器
[[Microduino-10DOF]]
==文档==
百度盘地址:
【'''[[media:BalanceCar Microduino-master.rar |平衡车代码]]'''】

github地址：
[https://github.com/Microduino/BalanceCar_Microduino BalanceCar_Microduino]

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

打开Arduino IDE编程软件，点击 【文件】->【打开】
[[File:Dl1.jpg||600px|center]]
浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开
[[File:Balancecaropen1.jpg||500px|center]]


[[File:Balancecaropen2.jpg||600px|center]]
点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序
[[File:WiFiStationopen4.jpg||600px|center]]


==烧录程序==
===下载源码===
先从以下地址下载源程序
'''[https://github.com/Microduino/BalanceCar_Microduino BalanceCar_Microduino]'''
===堆叠模块===
*将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），再通过USB数据线把Microduino-USBTTL模块与电脑连接起来。
===配置环境===
*打开Arduino IDE for Microduino环境，（建参考：'''[[AVR核心：Getting started/zh]]'''）
*确认“板卡”与“处理器”选择无误，并选中对应端口号（COMX）

===下载程序===
*打开你所下载的项目中“BalanceCar_Microduino.ino”程序
*点击“”按钮下载程序到核心板上。



==平衡车搭建==
*'''Step1''':首先将A1和A2对接,然后将B1和B2插在A1两端
[[File:Balancestep1.jpg||600px|center]]
*'''Step2''':将电机与C1对接，再用螺丝帽固定好
[[File:Balancestep2.jpg||600px|center]]
*'''Step3''':将第一步与第二部的结果拼接在一起,用B1进行固定
[[File:Balancestep3.jpg||600px|center]]
*'''Step4''':将轮胎用螺丝帽和螺丝固定在电机上
[[File:Balancestep4.jpg||600px|center]]
*'''Step5''':将电池模块放在图中指定位置,分别用C2和C3接在对应位置,用C4,A3,C5固定
[[File:Balancestep5.jpg||600px|center]]
*'''Step6''':将Core+,USBTTL,10DOF和底板进行拼接再用螺丝和螺丝帽固定好,最后使用传感器线和电池接入如图位置.
[[File:Balancestep6.jpg||600px|center]]
注意：在连接电机线的时候要注意方向，并稍微用力才能插入电机的接口，请按照图中方向插接。
[[File:Balancemotor.jpg||200px|center]]
====Joypad搭建====
*'''Step 1'''：将Microduino-TFT从Microduino-Joypad面板后面卡进Microduino-Joypad面板上，用尼龙螺丝固定，注意Microduino-TFT安装方向
[[File:Joypadstep1.jpg|center|600px]]
'''注意：接不同的电池需要拨动中间的开关，在图中已有标志。需要先拨开关再接入电池，否则会影响使用。'''
[[File:Joypadstep1_1.jpg|center|800px]]
*'''Step 2'''：将传感器接线插在Microduino-TFT的接口上
[[File:Joypadstep2.jpg|center|600px]]
*'''Step 3'''：首先分别将两个摇杆按键、电池、四个白色按键放入对应位置，之后将连接好天线的nRF模块和Core装到Joypad底板上
[[File:Joypadstep3.jpg|center|600px]]
*'''Step 4'''：将Microduino-TFT传感器接线的另一头接到底板上的相应位置，之后将长版螺丝帽放到四个角的相应位置
[[File:Joypadstep4.jpg|center|600px]]
*'''Step 5'''：将Joypad的表壳和底板使用螺丝和螺丝帽固定好
[[File:Joypadstep5.jpg|center|600px]]
*'''Step 6'''：组装完成后将天线上的贴纸撕下，将天线贴在底板背面的任意位置，至此Joypad组装完毕
[[File:Joypadstep6.jpg|center|600px]]
*自平衡小车和Joypad测试

====Joypad调试====
*按键对应
在打开Joypad之后的4秒左右时间之内按下Key1(下方最左侧的按键)，会进入设置(Config)模式
[[File:Step1进入设置.jpg|600px|center|]]
*进入设置模式
按照图中的颜色，从左至右对应为Key1~Key4
[[File:Step1按键对应.jpg|600px|center|]]
注意：必须在进入操作界面前进入（4S左右时间）。若未进入则重启进入'''
*摇杆校准
按动Key3和Key4使光标上下移动，Key1为返回，Key2为确认
选择第一项Joystick Config进入摇杆设置模式
继续选择Joystick Correct进入摇杆校准模式。
进入之后会显示如图中第三张图所示的界面，初始状态为两个十字
此时摇动左右摇杆至最上，最下，最左，最右四个极限状态
（推荐操作方式：将摇杆摇动一圈）
摇动之后会看到十字的四个方向出现圆圈，圆圈扩大到最大状态证明已经是摇杆的极限位置
校准之后按Key2确认并返回上一页面
[[File:Step2摇杆校准.jpg|600px|center|]]
*选择控制模式
按Key1回到主界面，选择第二项Protocol Config进入模式选择
选择第一项Mode，之后选择nRF24即robot控制模式，按下Key2确认并返回
[[File:Step3设置Robot模式.jpg|600px|center|]]
*设置通信信道
返回二级菜单，选择nRF24 Channel按下Key2确认
选择70，它是与Robot_Microduino.ino中nRF24的配置函数设置相对应的
[[File:Step4通信通道设置robot.jpg|600px|center|]]

==注意问题==
*下载程序时候最好只叠加core（core+）和USBTTL，虽然本次搭建涉及的nRF24不会引起冲突，但是别的通信模块有时会造成串口冲突，养成好习惯。
*Core+要叠在nRF24，USB的底下，紧贴ROBOT板。
*锂电池正负极别接错了，否则会烧坏电路。	
*调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池
==平衡车程序说明==
nrf设置部分
<source lang="cpp">
  //nRF==============================
  SPI.begin();		//初始化SPI总线
  radio.begin();
  //此处与Joypad上设定的通道数对应
  network.begin(/*channel*/ 70, /*node address*/ this_node); 

  digitalWrite(LED, HIGH);
  Serial.println("===========start===========");
</source>
端口设定
<source lang = "cpp">
#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
</source>
与遥控器进行数据交换的数组
<source lang="cpp">
struct receive_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;
};
</source>
PID控制实现
<source lang="cpp">
// PD的实施。 DT是毫秒
float stabilityPDControl(float DT, float input, float setPoint,  float Kp, float Kd)
{
  float error;
  float output;
  error = setPoint - input;
  // Kd的两部分实施
  //仅使用输入（传感器）的一部分而不是设定值输入输入（T-2）最大的一个
  //而第二个使用该设定值，使之更有点侵略性设定点设定点（T-1）
  output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; // + 错误 - PID_error_Old2
  //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);
}
// PI实现。 DT是毫秒
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);
}
</source>
PID算法-平衡控制
<source lang="cpp">
void robot()
{
  //===============================================================
  timer_value = millis();

  // 新的DMP定位解决方案
  fifoCount = mpu.getFIFOCount();
  if (fifoCount >= 18)
  {
    if (fifoCount > 18) // 如果我们有一个以上的数据包，我们采取简单的路径：丢弃缓冲区
    {
      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();  // 我们始终复位FIFO
    // 我们计算估计机器人的速度
    actual_robot_speed_Old = actual_robot_speed;
    actual_robot_speed = (speed_m[1] - speed_m[0]) / 2; // 正面：前锋
    // 角速度角度调整角度调整旧
    int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0; 
    // 我们利用机器人速度（T-1）或（T-2），以补偿该延迟
    int16_t estimated_speed = actual_robot_speed_Old - angular_velocity;     
    //估计速度估计过滤滤速
    estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05; 
   //目标角速度PIC ONTROL dt的速度估计过滤油门Kp_thr Ki_thr
    target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
    //有限的输出  目标角度约束目标角度最大目标角度最大目标角度
    target_angle = constrain(target_angle, -max_target_angle, max_target_angle); 

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

    //我们整合输出（加速度）
    control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
    control_output = constrain(control_output, -800, 800); // 限制最大输出控制
    // 控制的转向部分的输出直接注射
    motor1 = control_output + steering;
    motor2 = -control_output + steering;   //马达2反转
    // 限制最大速度
    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;
    }

  }
}
</source>
遥控器数据接收后选择数据控制平衡车的前后与左右
<source lang="cpp">
    float * _i = _speed;
      //CH1,CH0,CH7对应之前的发送/接收部分定义的数组
    _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);    //接收请求时序赋值
 </source>

==Joypad程序及说明==
Joypad部分
def.h中
定义了
<source lang = "cpp">
uint8_t nrf_channal = 70;  //0~125
</source>
nrf_channal为nrf通信的通道，joypad和Cube小车的代码中都会有该定义
当通道一致时则Joypad可与Cube小车成功连接。
在小车代码中会有如下程序段
<source lang = "cpp">
 //nRF==============================
  SPI.begin();		//初始化SPI总线
  radio.begin();
  network.begin(/*channel*/ 70  , /*node address*/ this_node);
</source>
在data.h中
<source lang = "cpp">
  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>
8位数组outBuf表示Joypad发出的8位数据，0位为右摇杆左右，1为右摇杆上下，2为左摇杆左右，3位左摇杆上下,4~7位对应AUX0~4

在nrf.h中
<source lang = "cpp">
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;
};
</source>
此处定义的send_a结构体位对应的0位要发送的数据

==视频==