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

{| style="width: 800px;"
|-
|
==概述==
*项目名称：Microduino开源平衡车
*目的：DIY一台属于自己的两轮自平衡小车
*难度：中
*耗时：2小时
*制作者：
*简介:
两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本教程将指导读者基于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||固定支撑
|-
|M2尼龙螺丝||4||固定底板
|-
|M2尼龙螺柱||4||固定底板
|-
|M2尼龙螺母||4||固定底板
|-
|短铜柱||8||固定电机
|-
|M4金属螺丝||2||固定车轮
|-
|2s锂电池（7.4V） ||1||供电
|-
|Micro-USB数据线 ||1||串口通信，下载程序
|-
|车轴连接器 ||2||连接电机轴和车轮
|-
|车轮 ||2||结构
|-
|步进电机 ||2||驱动车轮
|-
|步进电机线 ||2||连接电机和驱动板
|}
[[File:平衡车物料.jpg||1000px|center]]

==程序下载==
从以下地址下载源程序
'''[https://github.com/Microduino/BalanceCar_Microduino BalanceCar_Microduino]'''

==下载程序==
===堆叠模块===
*将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），再通过USB数据线把Microduino-USBTTL模块与电脑连接起来。
===配置环境===
*打开Arduino IDE for Microduino环境，（搭建参考：'''[[AVR核心：Getting started/zh]]'''）
*点击 【文件】->【打开】
[[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]]




==平衡车搭建==
==='''Step1'''===
*按图1-1所示将'''结构-A1'''和'''结构-A2'''对接
[[File:Balancestep1-1.jpg||250px|center]]
*按图1-2所示将'''结构-B1'''和'''结构-B2'''插在'''结构-A1'''两边
[[File:Balancestep1-2.jpg||250px|center]]
*步骤1完成后组成'''小车骨架'''
[[File:Balancestep1-3.jpg||250px|center]]
==='''Step2'''===
*按图2-1所示将2个'''步进电机'''与'''结构-C1'''对接，再用'''铜柱'''固定好
'''注意：'''注意图中所示电机接口方向，两个电机接口应该对称安装。
[[File:Balancestep2-1.jpg||350px|center]]
*步骤2完成后组成'''电机组件'''
[[File:Balancestep2-2.jpg||350px|center]]

==='''Step3'''===
*按图3-1将'''电机组件'''卡到'''小车骨架'''上，注意电机接口需要对应'''小车骨架'''的缺口，请按图中所示方向将'''电机组件'''卡入卡槽内
[[File:Balancestep3-1.jpg||350px|center]]
*'''电机组件'''卡到位后，将'''结构-B1'''卡入卡槽缺口，卡紧'''电机组件'''
[[File:Balancestep3-2.jpg||350px|center]]
*步骤3完成后组成'''小车地盘'''
[[File:Balancestep3-3.jpg||250px|center]]

==='''Step4'''===
*按图4-1所示将'''车轴连接器'''固定在'''步进电机'''轴上，然后将'''车轮装在'''车轴连接器'''上，然后用'''螺丝'''固定紧
[[File:Balancestep4-1.jpg||350px|center]]
*步骤4完成后小车如图4-2所示
[[File:Balancestep4-2.jpg||250px|center]]

==='''Step5'''===
*按图5-1所示将'''2s电池'''放在'''小车地盘'''的槽内,然后将'''结构-C2'''插在'''小车地盘'''上面，将'''结构-C3'''插在'''小车地盘'''的底部
[[File:Balancestep5-1.jpg||350px|center]]
*按图5-2说是用'''结构-A3'''、'''结构-C4'''和'''结构-C5'''卡紧小车上的部件
[[File:Balancestep5-2.jpg||350px|center]]
*步骤5完成后小车如图5-3所示
[[File:Balancestep5-3.jpg||350px|center]]

==='''Step6'''===
*将[[Microduino-Shield Stepper/zh]]用'''尼龙螺丝'''、'''尼龙螺柱'''和'''尼龙螺母'''固定在'''小车地盘'''顶部
*将2.4G天线接到[[Microduino-Module nRF/zh]]上
*然后将[[Microduino-Core+/zh]],[[Microduino-Module nRF/zh]]和[[Microduino-Module Motion/zh]]叠加到[[Microduino-Shield Stepper/zh]]上
[[File:Balancestep6-1.jpg||250px|center]]
*然后按以下示意图用'''步进电机线'''连接'''步进电机'''和[[Microduino-Shield Stepper/zh]]，请注意两个'''步进电机线'''的接口位置。
[[File:Balancestep6-2.jpg||250px|center]]
'''注意'''：在连接电机线的时候要注意方向，并稍微用力才能插入电机的接口，请按照图中方向插接。
[[File:Balancemotor.jpg||200px|center]]
==='''Step7'''===
*按图7-1所示将'''2s电池'''插入[[Microduino-Shield Stepper/zh]]的电池接口，小车即上电工作，扶起小车，小车即能保持平衡不倒
[[File:Balancestep7-1.jpg||250px|center]]

==Joypad遥控指南==

*搭建过程可参考以下页面：'''[https://www.microduino.cn/wiki/index.php/Microduino-Joypad_Getting_start/zh Joypad使用指南]'''

*'''Joypad通讯模式配置'''*
*模式配置可参考以下页面：'''[https://www.microduino.cn/wiki/index.php/Microduino-Joypad_Getting_start/zh#.E9.80.9A.E8.AE.AF.E6.A8.A1.E5.BC.8F 通讯模式配置]'''
**当我们使用[[Microduino-Module nRF/zh]]作为'''通讯模块'''时，Joypad对应选择'''nRF模式'''即可

*'''Joypad电池使用说明'''*
*特别要注意的是电池使用说明：'''[https://www.microduino.cn/wiki/index.php/Microduino-Joypad_Getting_start/zh#Step-2_.E7.94.B5.E6.B1.A0.E9.80.9A.E7.94.B5 电池使用说明]'''

==代码说明==
*在“user_def.h”文件是我们的配置文件
*以下代码可以配置nRF模式下通道，需要保证和Joypad遥控器一致
*Joypad的nRF模式通道配置可参考：[https://www.microduino.cn/wiki/index.php/Microduino-Joypad_Getting_start/zh#nRF.E6.A8.A1.E5.BC.8F.E9.80.9A.E9.81.93.E9.85.8D.E7.BD.AE nRF模式通道配置]
<source lang = "cpp">
#define NRF_CHANNEL 70  //nRF通道
</source>


*以下代码可以配置油门和转向对应通道
*对应通道说明可参考：[https://www.microduino.cn/wiki/index.php/Microduino-Joypad_Getting_start/zh#Step-5_.E9.80.9A.E9.81.93.2F.E6.93.8D.E4.BD.9C.E8.AF.B4.E6.98.8E 通道/操作说明]
<source lang = "cpp">
#define CHANNEL_THROTTLE  2 //油门通道
#define CHANNEL_STEERING  1 //转向通道
</source>


*以下代码可以修正两轮的转速比例
**值得范围是-1到1之间
**设置成-1，是最大转速比反转
**设置成1，是最大转速比正转，
**如果小车不能走直线，应该将较慢的一边轮子的转速比例值降低
<source lang = "cpp">
#define motor_fixL 1  //速度修正 -1到1之间
#define motor_fixR 1  //速度修正 -1到1之间
</source>


==注意问题==
*下载程序时候最好只叠加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位要发送的数据

==视频==