引言
莱罗三角形也叫鲁洛克斯三角形，它是这样得到的：先画正三角ABC，然后以正三角形ABC的三个顶点为圆心，边长长为半径画弧得到的图形，如下图所示，将一个曲线图放在两条平行线中间，使之与这两平行线相切，则可以做到：无论这个曲线图如何运动，只要它还是在这两条平行线内，就始终与这两条平行线相切。这个定义由十九世纪的德国工程师弗兰茨·勒洛命名。莱洛三角形在生活中应用很多，比如转子引擎等，这次我们组选择莱洛三角来做一个自平衡装置，他的平衡原理就是角动量守恒，当整体有向左边倒的时候，动量轮逆时针加速，根据角动量守恒，电机应该连着整个系统顺时针加速从而抵消向左边倒的趋势，向右边倒时原理相同。
整体硬件概述
在硬件设计部分，我们小组通过组内讨论，分析选题要求，在主控的选择上有两种方案：
- STC89C52RC作为51系列市场上最为常见的单片机之一，采用的是CMOS8位MCS-51内核，相比以往AT系列51单片机的4K内存增加了一倍，8位CPU使得芯片更为灵巧，在嵌入式控制应用中更有效，对芯片要求不高的场合下均可使用。
- LGT8F328P是一款国产指令集兼容 AVR，Atmega328P的芯片，这是一颗增强8位RISC内核，针对原来架构的做了优化，指令执行速度快了一些，同时优化了flash部分，增加定时器3，内置晶振，增加DAC输出，主频最高可到32M，功耗更低,能够99%兼容Arduino IDE。
结论：最终决定采用价格更为低廉，开发便捷，主频较高的8位增强型AVR MCU，LGT8F328P作为主控单片机，以满足对自平衡的计算需求。
其他硬件选型如下：
整体硬件采用模块化设计，驱动部分主体驱动电机采用一个12V AB信号反馈 nidec无刷减速电机，该FOC电机自带控制系统板，可以轻松实现PWM转速控制，方便后续开发，采用一个规格：42X30X8mm的不锈钢轴承作为该自平衡系统的动量轮，并采用挤压环连接，保持其稳固。供电部分，由于需要自平衡，首先排除了外接电源，因此采用一块400mah的1S航模锂电池作为主供电，由于锂电提供的电压为3.7V我们需要一个3.7V转5V的升压模块来保证系统稳定运行，同事，我们的驱动电机采用的12V供电，也需要一个3.7V转12V的升压模块来维持电机正常工作。姿态识别部分，我们采用了MPU6050作为识别传感器，MPU6050内部一个可扩展的数字运动处理器可以将欧拉角以四元数的形式输出这样就不需要我们对陀螺仪和加速度传感器采集的原始数据进行融合和滤波，调试部分，引出单片机的串口后采用BT04-E蓝牙模块，体积小巧，便于调试。下图为整体硬件框图 ：
整体硬件选型完后，再PCB的设计上我们选择了简洁的原则，使用2层板，采用了大量模块进行表贴，整体模块区域分明，板框采用异形莱罗三角设计，适配本项目后期壳料模型。
软件控制部分概述
程序部分分为了调试模式和正常运行模式，可以通过修改程序的ifdef定义进行切换，在调试模式时，在各个模块初始化后，对姿态传感器的姿态进行读取，并通过串口进行打印输出，我们可以通过USB转TTL模块连接在预留的串口排针上，直接向上位机输出姿态值进行调试，也可将蓝牙模块连接在预留的串口排针上，在与手机配对后进行可以查看姿态值，方便调试PID参数，关于PID算法如下为PID算法模拟表达式：
比例项是纠正偏差的主力，越远离偏差绝对值就越大，快速把偏差纠正回来。积分项和以往的状态有关，面积的绝对值越大它的绝对值就越大，它的作用是消除累计偏差。微分项跟斜率有关，比较难解释，总的来说它的作用是：当目标靠近设定值时加速它靠近，当目标远离设定值时阻止它远离。因此微分可以增加系统稳定性，因为到达目的之后，离开会受到阻碍。在本项目中我们要对角度进行PID计算，根据经验，角度的控制要把动态误差控制到很小（不然倾斜角度过大回调所需动量超过了电机极限系统就会崩溃）所以PD的控制作用很大，后面的调参也主要是对这两个参数，I可有可无。
PID输出做差后的值作为PWM波占空比用来驱动电机，这里我们不需要纠结其中数学原理，只需要明白测量角度和目标角度的差值作为角度控制输入，电机编码器10ms读到的值（这个相当于电机转速）作为速度控制输入，这样我们得到的两个输出，一个和系统倾斜角度有关一个和电机速度有关，他们共同决定了电机转速，后面我们只需要把PID参数调好就能让角度和电机速度对PWM占空比的作用得到适合的权重使系统稳定。当程序正常运行时，首先当检测到姿态处于初始状态，不在对应的平衡角度时，通过控制FOC电机正反转获得一定动量偏差，使莱洛三角开始摇摆，在处于接近平衡姿态时，加快FOC的转速来保持平衡，当处于平衡状态的设定值时，控制FOC电机匀速转动，使动量守恒，达到平衡的目的，整个系统程序框图如下图所示：
整个自平衡系统逻辑相对比较清晰，我们采用Arduino IDE进行程序的编写整个编程过程需要软硬件相互配合调试，我们组决定采用模块化编程的思路，通过几个模块的填充来完成对所需功能的满足。PID控制代码如下：
extern double GyroZ_lowout; //角速度
extern double kalAngleZ;
//直立环参数 A100;0;14;250;0.01;5; A100;0;14;250;0.01;5;
float Vertical_Kp = 100.0,Vertical_Kd = 14.0;
float Angle_mid = 0;//机械中值
int16_t PWM_out,Vertical_PWM,Velocity_PWM;
//速度环参数
float Velocity_Kp = 250.0, Velocity_Ki=0.01;
double Encoder_Err=0, Encoder_Sum=0;
double Encoder_lowout=0;
int16_t PIDControl(){
/****************************************************************
速度环PI控制器：
****************************************************************/
//1.计算速度偏差 Encoder_Err
Encoder_Err = 16666.6/speedTCounter;
//Stimer = micros();
//2.对速度偏差进行低通滤波//low_out = (1-a)*Ek+a*low_out_last;
Encoder_lowout = 0.3*Encoder_Err + 0.7*Encoder_lowout;
//Encoder_lowout = Encoder_Err;
//3.对速度偏差,积分出位移
Encoder_Sum += Encoder_Err;
if(Encoder_Sum>10000){
Encoder_Sum = 10000;
}
else if(Encoder_Sum<-10000){
Encoder_Sum = -10000;
}
//4.速度环控制输出——期望角度
Velocity_PWM = (int16_t)Velocity_Kp*Encoder_lowout+Velocity_Ki*Encoder_Sum;
/****************************************************************
直立环PD控制器：
****************************************************************/
Vertical_PWM = (int16_t)Vertical_Kp*(kalAngleZ - Angle_mid) + Vertical_Kd * GyroZ_lowout;
PWM_out = Vertical_PWM + Velocity_PWM;
if(PWM_out > 790){
PWM_out = 790;
}
else if(PWM_out < -790){
PWM_out = -790;
}
return PWM_out;
}电机驱动模块的软件设计
对于FOC驱动模块，首先我们需要了解FOC驱动控制原理，FOC(Field Oriented Control)，直译即为磁场定向控制，又称为磁场矢量控制(VC, Vector Control)。FOC特点：通过精确地控制磁场大小与方向，使得电机的运动转矩平稳、噪声小、效率不高，且具有高速的动态响应。在本项目中，电机因为板载了FOC驱动电路，因此不需要太多数学上的计算，我们需要给驱动板PWM信号进行调速，给定ENA,ENB信号控制正反转即可完成驱动模块的控制。
对于PWM的发生我们根据芯片的数据手册，决定采用定时器1快速模式输出FPWM，快速模式可以产生高频PWM，快速 PWM 模式和其他 PWM 模式不同在于它是单向操作。计数器从最小值 0x00 累加到 TOP 后又回到 BOTTOM 重新计数。当计数值 TCNT0 到达 OCR0x 或 BOTTOM 时，输出比较信号 OC0x 会被置位或清零，取决于比较输出模式 COM0x 的设置，波形的频 率可用下面的公式来计算： foc0xfpwm = fsys/(N*(1+TOP)) 其中，N 表示的是预分频因子（1，8，64，256 或者 1024）。本项目我们需要可调速度，响应快的PWM，用这项功能可以满足，设置计数器上限，输出频率20kHz用作比较器，OCR1B设置PWM占空比 OCR1B = 0电机转速为0。定时器1还具有死区时间可调的功能，如图所示OC1A 和 OC1B 的输出波形将在 B 通道比较输出所产生的波形基础上插入设定的死区时间，时间的长度为 DTR1 寄存器的计数 时钟数所对应的时间值。但本项目中无需使用。
LGT8FX8P单片机定时器1输出PWM时序图
关于启停，刹车，方向的控制，通过配置单片机不同IO口的高低电平，我们就可以在对驱动板进行控制，驱动板EN引脚用来控制电机启停，当我们给驱动板的EN引脚配置低电平时，电机被关闭，反之则被开启，ENA和ENB引脚分别连接了编码器A和B，A用来检测正反转并测速，B用来检测正反转并测速，本项目我们采用编码器B来实现功能，我们给连接编码器B所对应的引脚，在初始化时配置成上拉输入当接收到高电平脉冲时，说明电机正转，脉冲个数代表了速度的快慢，当接收到低电平脉冲时，说明电机反转，脉冲个数代表了速度的快慢，这样就可以对电机的运行状态进行检测，连接DIR的引脚通过单片机输出高低电平实现控制电机正反转，连接BK引脚为刹车引脚当设置为低电平时，刹车关闭，反之，开启刹车，通过编写程序，我们可以实现电机控制并封装，从而应用于PID控制的执行机构和反馈机构。
MPU6050姿态传感器模块的软件设计
MPU6050是一个6轴姿态传感器（3轴加速度计和3轴陀螺仪传感器），可以测量芯片自身X、Y、Z轴的加速度、角度参数，通过数据融合，可以得到姿态角。对于其X、Y、Z三轴的定义如下图2-3所示，横向的是x轴，纵向的是Y轴，垂直与芯片的是Z轴。
在MPU6050中，X、Y、Z轴都具有一个加速度计，假设芯片里有6个测力的秤组成一个正方体，正方体内部放一个大小正好的单位质量小球，小球压在一个面上，就会产生对应轴的数据输出。如果压在上面为正值，压在下面为负值，6个面测的力就是3个轴的加速度值。MPU6050内部I2C时序如图2-3-1上图所示，MPU6050内部陀螺仪寄存器如下图所示，我们需要通过I2C协议，来读取相应寄存器，从而获取我们的姿态值，给PID控制器传递输入参数，首先配置的是采样分频设置寄存器。
MPU6050 I2C通信时序陀螺仪寄存器
本项目姿态ADC 采样率1khz即可，因此配置寄存器写入I2C数据的第一位为7，即可配置采样频率为1000Hz - 8kHz/(7+1) = 1000Hz，其次，需要配置陀螺仪传感器和加速度传感器的输出加速度与角速度以便我们计算角度，如图所示为MPU6050内部陀螺仪控制寄存器。
MPU6050陀螺仪寄存器
我们在程序中调用Adruino IDE 中的I2C库一次性写入四个寄存器，while (i2cWrite(0x19, i2cData, 4, false)); 将该寄存器配置为具有 X 轴陀螺仪参考的 PLL 用while (i2cWrite(0x6B, 0x01, true)); 语句即可实现配置完后需要等待传感器稳定。代码如下：
#include <Wire.h>
#include "Kalman.h"> // Source: https://github.com/TKJElectronics/KalmanFilter
#define gyroZ_OFF 0
Kalman kalmanZ;
double accX, accY; //加速度
double gyroZ, GyroZ_lowout; //角速度
double kalAngleZ; //Calculated angle using a Kalman filter 卡尔曼滤波计算Z轴角度
double gyroZangle; // Angle calculate using the gyro only
uint8_t i2cData[14]; //Buffer for I2C data
uint32_t Atimer; //计算间隔时间dt，计算角度
double pitch,last_pitch; //
double acc2rotation(double x, double y);
/****************************************************************
kalman mpu6050 init
****************************************************************/
void Mpu6050init(){
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±500deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68)
{ // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(1000); // 延时等待传感器稳定
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
double pitch = acc2rotation(accX, accY);
kalmanZ.setAngle(pitch); // Set starting angle
gyroZangle = pitch;
Atimer = micros();
}
void GetZAngle(){
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
double dt = (double)(micros() - Atimer) / 1000000.0; // Calculate delta time
Atimer = micros();
pitch = acc2rotation(accX, accY);
gyroZ = gyroZ / 131.0 + gyroZ_OFF; // Convert to deg/s
// if(abs(pitch-last_pitch)>100) //角度变化过大，重置卡尔曼滤波？？也可以用于判定：如果角度偏移过大，不具备平衡条件，停止摆动。
// kalmanZ.setAngle(pitch);
// GyroZ_lowout = 0.1*gyroZ + 0.9*GyroZ_lowout;
GyroZ_lowout = gyroZ;
kalAngleZ = kalmanZ.getAngle(pitch, GyroZ_lowout, dt);
last_pitch = pitch;
}
/* mpu6050加速度转换为角度
acc2rotation(ax, ay)
acc2rotation(az, ay) */
double acc2rotation(double x, double y)
{
double tmp_kalAngleZ = (atan(x / y) *57.29578);
if (y < 0)
{
return (tmp_kalAngleZ + 180);
}
else if (x < 0)
{
//将当前值与前值比较，当前差值大于100则认为异常
if (!isnan(kalAngleZ) && (tmp_kalAngleZ + 360 - kalAngleZ) > 100) {
//isNaN () 函数可确定值是否为非数字（Not-a-Number）。 如果该值等于 NaN，则此函数返回 true。 否则返回 false
//Serial.print("X<0"); Serial.print("\t");
//Serial.print(tmp_kalAngleZ); Serial.print("\t");
//Serial.print(kalAngleZ); Serial.print("\t");
//Serial.print("\r\n");
if (tmp_kalAngleZ < 0 && kalAngleZ < 0) //按键右边角
return tmp_kalAngleZ;
else //按键边异常处理
return tmp_kalAngleZ;
} else
return (tmp_kalAngleZ + 360);
}
else
{
return tmp_kalAngleZ;
}
}由于传感器采集的Z轴数据噪点较多，我么还要对其进行卡尔曼滤波来计算Z轴角度，卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。其有五个公式具体如下：
1.x^k-1和x^k，分别为k-1时刻与k时刻的后验状态估计，是滤波结果之一，即更新后的结果，也叫最优估计。(因为不可能知道真实值，所以加一个hat，表示最优估计)
2.x^k ̅，表示k时刻的的先验状态估计，及根据k-1时刻的最优估计预测k时刻的状态。
3.Pk-1和Pk，分别为k-1时刻与k时刻的后验估计协方差(即xk-1和的协方差，表示状态的不确定性)，为滤波结果之一。
4.Pk ̅，为k时刻的先验估计协方差(x^k ̅的协方差)。
5.Kk，为卡尔曼增益。
好在Adruino IDE 中有卡尔曼滤波算法的库文件，我们无需太过关注数学上的原理，调用库文件传入传感器参数，如 给有一个变量赋kalmanZ.getAngle(pitch,GyroZ_lowout, dt)；即可将Z轴角度赋值在那个变量上，以便我们在主程序中调用。kalman.cpp代码如下：
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
#include "Kalman.h"
Kalman::Kalman() {
/* We will set the variables like so, these can also be tuned by the user */
Q_angle = 0.001f;
Q_bias = 0.003f;
R_measure = 0.03f;
angle = 0.0f; // Reset the angle
bias = 0.0f; // Reset bias
P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.0f;
};
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
float Kalman::getAngle(float newAngle, float newRate, float dt) {
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
float S = P[0][0] + R_measure; // Estimate error
/* Step 5 */
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
float y = newAngle - angle; // Angle difference
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
};
void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle
float Kalman::getRate() { return this->rate; }; // Return the unbiased rate
/* These are used to tune the Kalman filter */
void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
float Kalman::getQangle() { return this->Q_angle; };
float Kalman::getQbias() { return this->Q_bias; };
float Kalman::getRmeasure() { return this->R_measure; };PID控制的软件设计
根据PID控制算法公式，我们通过PID控制将MPU6050传感器输出的数据以及正交编码器B输出的数据作为整个控制系统的输入，将PWM和DIR作为整个控制系统的输出，根据公式我们要定义我们所需要控制环，直立环和速度环，直立环是用来保持整个系统平衡的，即对整体系统角度进行控制使其维持在一定范围内保持不变，经测，处于30°，150°，270°时为我们所期望的平衡点，我们通过直立环来控制其稳定，这里直立环采用PD控制，PD调节器，调节偏差快速变化时使调解量在最短的时间内得到强化调节，有调节静差，适用于大滞后环节；速度环是在直立环的控制之下，来控制我们系统的执行机构FOC电机，因此我们需要响应块的PI控制，PI调节器，兼顾快速性，减小或消除静差（I调节器无调节静差），满足我们的需求。针对速度环，我们要在代码中散步处理1.计算速度偏差 Encoder_Err，2.对速度偏差进行低通滤波，3.对速度偏差,积分出位移，4.速度环控制输出——期望角度。针对直立环我们只需要判断输入的角度值是否在我们设定的直立角范围内即可，如图2-4所示为该系统的程序结构框图：
对于整体参数的整定，我们根据工程经验确定了在直立环中Vertical_Kp = 100.0，Vertical_Kd = 14.0，在速度环中Velocity_Kp = 250.0，Velocity_Ki=0.01，只需要确定该系统平衡时的三个机械中值即可实现该闭环控制，摇摆程序的实现，其系统框图如图2-4-1所示，我们需要在PWM输出的子程序中加入对姿态的判断，如果出于90°等非平衡角度，则进入摇摆程序，其具体控制模式如图，都是通过传感器给出的角度输出PWM，当输出过冲时，减小输出，输出不足时，增大输出，从而产生偏心力，能够使莱洛三角左右摇摆，以接近平衡的点，在进行PID控制保持平衡。
摇摆程序执行框图：
电机控制代码如下：
int32_t speedTCounter = 0; //用于计算电机转速
int32_t speedTCounterLast;
float gyroZLowoutLast = 0;
int16_t tempPWM_out = 0; //PWM过渡变量
String inputString = ""; // a String to hold incoming data
int delayTime = 5;
int sendFlag = 1;
/****************************************************************
LGT8F328P单片机初始化设置函数
****************************************************************/
void MPUInitial(){
pinMode(Motor_EN_BIT, OUTPUT); //设置电机使能引脚为输出模式；
digitalWrite(Motor_EN_BIT, LOW); //关闭电机
pinMode(Motor_DIR_BIT, OUTPUT); //设置电机方向引脚为输出模式；
pinMode(EncoderB_DIR, INPUT_PULLUP); //设置编码器B相引脚为输入上拉模式；
pinMode(Motor_BK_BIT, OUTPUT); //设置电机刹车引脚为输出模式；
digitalWrite(Motor_BK_BIT, LOW); //关闭刹车
/****************************************************************
设置Timer1为PWM输出定时器，输出引脚Pin10,输出PWM控制电机转动
****************************************************************/
cli(); // 禁止全局中断
pinMode(Motor_PWM_BIT, OUTPUT); //设置PWM引脚Pin3为输出模式
//设置Timer1为快速PWM模式，计数上限OCR1A；发生比较匹配时OC1B清零（低电平），计数到下限时OC1B置位（高电平）；系统时钟1分频
TCCR1A = (1<<COM1B1) | (1<<WGM11) | (1<<WGM10);
TCCR1B = (1<<WGM13) | (1<<WGM12) | (1<<CS10); // 0b00011001;
OCR1A = 799; //设置计数器上限，输出频率20kHz
OCR1B = 0; //用作比较器，设置PWM占空比 OCR1B = 0电机转速为0
/****************************************************************
初始化外部中断0，下降沿触发，调用counter()函数
****************************************************************/
pinMode(EncoderA, INPUT_PULLUP); //设置编码器A相引脚为输入上拉模式；
attachInterrupt(0, counter, FALLING); //设置编码器A中断绑定到counter()函数中；
sei(); // 允许全局中断
}
/****************************************************************
控制电机转速
****************************************************************/
void PWM_OutPut(){
//根据当前角度判断是进入平衡程序还是摇摆程序
//PID平衡程序
if(abs(kalAngleZ-MID_ANGLE1)<LIMIT_ANGLE1)
{
//确定机械中值
Angle_mid = MID_ANGLE1;
//将数据输入闭环控制中，计算控制输出量
tempPWM_out = PIDControl();
delay(delayTime);
}
else if(abs(kalAngleZ-MID_ANGLE2)<LIMIT_ANGLE1)
{
Angle_mid = MID_ANGLE2;
tempPWM_out = PIDControl();
delay(delayTime);
}
else if(abs(kalAngleZ-MID_ANGLE3)<LIMIT_ANGLE1)
{
Angle_mid = MID_ANGLE3;
tempPWM_out = PIDControl();
delay(delayTime);
}
else //摇摆程序
{
if(abs(GyroZ_lowout)<1000)
{
if( GyroZ_lowout > 0 ) //判断转动方向
{//逆时针摆动
if((GyroZ_lowout - gyroZLowoutLast) > 0) //通过两次角速度的差值，判断当前时加速还是减速
{//加速运动
if( GyroZ_lowout < 5 )
{
if(abs(tempPWM_out)<100){
tempPWM_out = -100;
}
}
else
{
tempPWM_out = tempPWM_out -6;
}
}
else
{//减速运动
tempPWM_out = tempPWM_out -6;
}
}
else//顺时针摆动
{
if((GyroZ_lowout - gyroZLowoutLast) < 0){//加速运动
if( GyroZ_lowout > -10 )
{
if(abs(tempPWM_out)<100)
{
tempPWM_out = 100;
}
}
else{
tempPWM_out = tempPWM_out + 6;
}
}
else
{
tempPWM_out = tempPWM_out + 6;
}
}
if(tempPWM_out > 150){
tempPWM_out = 150;
}
else if(tempPWM_out < -150){
tempPWM_out = -150;
}
}
gyroZLowoutLast = GyroZ_lowout;
}
//将控制输出加载到电机上，控制电机输出
if(tempPWM_out>0){//电机正转-顺时针
digitalWrite(Motor_DIR_BIT, LOW);
OCR1B = tempPWM_out;
}
else{
digitalWrite(Motor_DIR_BIT, HIGH);
OCR1B = -tempPWM_out;
}
}BT04-E蓝牙传输模块的软件设计
该模块为蓝牙3.0模块，搭载该模块通过串口转蓝牙输出姿态信息，方便我们PID调参，虽然该模块支持SPI和I2C通讯，但是由于单片机IO资源有限，选择采用UART串口通讯完成对姿态参数的传递，该模块可直接将串口信息通过蓝牙发出，接收端接收后可直接读取，四个信号引脚用于实现 UART 功能。当 DX-BTO4-E 连接到另一个数字设备时，UART RX和UART TX在两个设备之间传输数据。其余两个引脚 UART CTS和 UART RTS 可用于实现 RS232 硬件流控制，且均为低电平有效，即低电平时允许传输，高电平时停止传输.首先设置波特率，官方数据手册推荐波特率最小位2400bps，最大为115200bps，此处我们选择9600bps，在单片机初始化程序中设定，之后采用switch-case语句，根据返回发送标志的不同，发送不同数据，如下段程序所示，我们分别需要返回的是，姿态值，PWM值，Kp,Ki，串口接受数据，用于PID调试，可以使用蓝牙模块进行无线调试，上位机发送数据的格式：Axx;xx;xx;xx;xx;xx;(xx为参数)以满足在蓝牙小程序读取的需要，对于这些数据的获取可以在串口中断子程序中实现。程序如下：
void serialEvent() {//串口接受数据，用于PID调试，可以使用蓝牙模块进行无线调试，上位机发送数据的格式：Axx;xx;xx;xx;xx;xx;(xx为参数)，微信搜索“平衡小车蓝牙调试助手”，使用蓝牙进行无线调试
while (Serial.available()) {
static bool ReceiveFlag = false;
static int ReceiveNum = 0;
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString:
inputString += inChar;
if(inChar == 'A'){
ReceiveFlag = true;
}
if((inChar == ';')&&(ReceiveFlag)){
ReceiveNum++;
}
// if the incoming character is a newline, set a flag so the main loop can do something about it:
if (ReceiveNum == 6) {
ReceiveNum = 0;
ReceiveFlag = false;
uint8_t len = inputString.length();
char temp[20] = "";
char buffer[len];
strcpy(buffer,inputString.c_str());
uint8_t m = 0;
uint8_t n = 0;
inputString = "";
if(buffer[0] == 'A'){
for (uint8_t i = 1; i < len; ++i) {
if (buffer[i] == ';') {
switch (n) {
case 0:
Vertical_Kp = atof(temp);
break;
case 1:
sendFlag = (int)atof(temp); //根据loop()函数，更改sendFlag的值，单片机返回不同的参数
break;
case 2:
Vertical_Kd = atof(temp);
break;
case 3:
Velocity_Kp = atof(temp);
break;
case 4:
Velocity_Ki = atof(temp);
break;
case 5:
delayTime = atof(temp); //相邻两次PID计算的延时参数
default:
break;
}
memset(temp,0,sizeof(temp));
m = 0;
n++;
}
else
{
temp[m] = buffer[i];
m++;
}
}
}
}
}
}机械结构概述
模型选择了B站UP DIY攻城狮的设计，在外形结构设计部分这部分我们采用solid works软件进行建模，模拟了结构设计的大致方向，结构为一个莱洛三角柱，中间挖去后留下边框。
建模完成后我们决定采用3D打印来完成该项目，最初我们选择了尼龙材料的3D打印材料来制作，打印了5小时后成功如下图3-2所示，材质很粗糙，材料也很脆，一摔便裂，无奈我们决定放弃采用尼龙材料，用砂纸打磨后将这一版转为调试用样品。
之后我们采用了树脂材质的打印件，整体相较用尼龙材料打印的要细腻许多，结构也有一定强度，在这一版我还改进了装置与桌面的接触，在三角形的边上开槽，用来放置橡皮筋增加与桌面的摩擦力，使其可以在光滑的桌面上摇摆，安装时发现挤压环安装过松，无法紧固动量环，最终采用热风枪加热的方法，使树脂材料处于半融化状态进行固定后才得以紧固。如图所示左边为调试样品，右边为最终成品，整体装置安装完毕至此整体装配已经就绪，各模块的安装到位，没有松动，短路，接触不良等现象发生，整个机械结构得到了很好的验证，说明了该机械结构的稳定，可靠，实用。
调试概述
程序的烧写
测试程序已经写好，接下来便是程序的下载，使用Arduino IDE单片机程序下载软件进行烧录。首先需要安装LGT8F328P单片机所需要的固件包，重启IDE后即可识别开发板，经过简单的设置，编译后按下开发板上的复位键即可完成程序下载。如图所示：
下载完debug调试程序后我们可以通过串口助手，或者Arduino IDE自带的串口调试终端观察不同姿态时的机械中值角度如图所示，但这样确定的机械中值准确度不佳，因此我们决定采用蓝牙，以无线形式确定机械中值。
经过反复调试，终于整定好三个姿态的机械中值，整定好的参数才实际运行时，能够保持平衡装置在平衡时，动量轮在匀速旋转，说明控制环已经确认完毕。
该装置启动后摇摆，到目标值即机械中值时能够保持平衡。
PID调参是一个高强度，重复性高，枯燥的过程，经过不断尝试与工程经验相结合，我们组最终能够调节到平衡状态且具体有一定的抗干扰能力。效果演示如下：
莱洛三角形演示视频
经过调试，我们使用了一根转接的充电器，主体为锂电池充放一体模块，输出用无极性端子连接锂电池，通过type-c进行供电为航模电池充电，如图4-5所示，充电无异常，发热量小，大约40分钟充满，有过充保护较为安全。
图4-5装置充电
结语
至此该装置调试完毕，实现了自平衡功能，是体积小巧，美观有趣的桌面摆件。
4208
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
