stm32项目
stm32项目介绍值平衡车
本文章学习借鉴于创客学院团队，以表感谢。教学视频
文章目录
- stm32项目
- 前言
- 一、平衡小车
- 平衡小车的功能介绍
- 平衡小车功能开发需求
- 平衡小车整体框架
- 小车环境数据采集进程
- 1. 平衡小车姿态信息介绍
- 2. 平衡小车项目工程框架搭建
- 3. Mpu6050姿态传感器驱动eMPL库使用
- 小车PID控制进程
- 1、PWM 直流电机驱动开发
- 测试电机小demo 。
- 2、正交码盘测试小车速度功能开发
- 编码器测试小demo
- 3、小车PID控制功能开发
- 1. 控制车模平衡
- 2. 控制车模速度-速度环PID调节
- 3. 控制车模方向-转向环PID
- 4、PID 计算函数（已整理）也可以csdn 上找类似的PID平衡车
- 超声波避障小车功能
- 巡线小车功能
- 遥控小车
- 总结
前言
提示：这里可以添加本文要记录的大概内容：
例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。
一、平衡小车
实物图如下所示各类平衡车
平衡小车的功能介绍
大容量电池供电
双马达电机驱动
平衡控制调节
速度控制调节
方向控制条件
上位机远程控制
平衡小车功能开发需求
- 平衡小车PID控制
直立、速度、方向 - 平衡小车遥控功能
无线通信功能、上位机App - 平衡小车避障功能
超声波测距 - 平衡小车巡线功能（线路轨迹运行）
摄像头CCD循迹 - 菜单显示及功能设置
OLED以及按键
效果如下图所示
平衡小车整体框架
平衡小车框架图如下
数据采集进程
我们在控制小车的前提是获取小车当前的状态数据，加速度，角速度，偏移角度等小车的姿态数据，进行数据采集进程。流程图如下：
小车用到的姿态传感器是mup6050姿态传感器。所以需要获取改姿态传感器的数据。
获取到这些数据后，可以通过PID进行控制、流程图如下：
避障模式应用到超声波测距功能
采集姿态----》进行控制-----》信息显示
OLED 显示模块 框架图如下：
设备流程完毕后，开始上机交互，交互进程框架图如下：
平衡小城用到的中断处理部分框图如下：
以上即平衡小车开发的框架图和流程图、便于我们的学习与管理。下面我们按部就班一步一步来实习上框图功能。
小车环境数据采集进程
1. 平衡小车姿态信息介绍
获取mpu6050 通过eMPL 库 获取需要的数据如下图：
mpu6050 姿态传感器之前介绍过，即欧拉角的获取。
不理解的可以看 点击查看，stm32 MPU6050 6轴姿态传感器的介绍与DMP的应用
俯仰角 （抬头）（ pitch ）： 围绕Y轴旋转的。
偏航角（摇头）（yaw）：围绕Z轴旋转的角度。
翻滚角 （翻滚）（roll）：围绕X轴旋转的角度
三轴传感器，x、y、z个方向的角度。如下图所示：
以上就是获取小车姿态的环境数据采集进程。
2. 平衡小车项目工程框架搭建
使用STM32CuBeMX 创建工程，完成配置。导出项目使用keil5软件打开运行。
3. Mpu6050姿态传感器驱动eMPL库使用
- 初始化MPU6050
- 移植MPU6050官方的eMPL库
- 获取原始数据以及姿态
注意 MPU6050 eMPL库 针对不同的平台需要调整 参考inv_mpu.c文件
MPU6050_eMPL 库文件夹目录如下图：
通过mpu_dmp_get_data（）获取加速度和角速度通过四元素quat 转化为我们需要的欧拉角
u8 mpu_dmp_get_data(void); //获取角，加速度，以及欧拉角数据
u8 mpu_dmp_init(void); //初始化MPU6050
u8 mpu_dmp_init(void)
{
u8 res=0;
IIC_Init(); //初始化IIC总线
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
// res=run_self_test(); //自检
// if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}
printf("init ok");
return 0;
}
/***************************************************************************************************************
*函数名:mpu_dmp_get_data()
*功能:得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
*形参:(struct _out_angle *angle):DMP解算得到的姿态
*返回值：0：成功/1：DMP_FIFO读取失败/2:数据读取失败
***************************************************************************************************************/
//声明这些全局变量，外部好使用
float pitch,roll,yaw;
u8 mpu_dmp_get_data(void)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
short gyro_dmp[3], accel_dmp[3], sensors_dmp;
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro_dmp, accel_dmp, quat, &sensor_timestamp, &sensors_dmp,&more)) return 1;
// printf("%d\n",dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more));
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors_dmp&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
//返回数据给全局变量 OutMpu
//加速度
OutMpu.acc_x = accel_dmp[0];
OutMpu.acc_y = accel_dmp[1];
OutMpu.acc_z = accel_dmp[2];
//角速度
OutMpu.gyro_x = gyro_dmp[0];
OutMpu.gyro_y = gyro_dmp[1];
OutMpu.gyro_z = gyro_dmp[2];
//计算机国赋值给偏航角，俯仰角，翻滚角
OutMpu.pitch = pitch;
OutMpu.roll = roll;
OutMpu.yaw = yaw;
return 0;
}
平衡车扶平 ，按下复位键
数据采集到这里就结束了。下面开始PID控制
小车PID控制进程
通过PID控制两个车轮，使其前后运动，调节车辆平衡，直立工作，利用速度差就控制转向。
1、PWM 直流电机驱动开发
控制板如何驱动电机，通过pwm 来控制
速度越快，扭矩越小，速度慢扭矩越大，丽日汽车起步需要低速费力
直流减速电机：损耗速度来增大扭矩
H桥电路，方便控制电机正反转
MC3386电机驱动芯片，该芯片包含有H桥电路
IN1和IN2控制正反转，IN1 输入高电平电机正转，IN2输入高电平电机反转。IN1和IN2 均是通过PWM电压来控制转速。
下图为PWM 周期
测试电机小demo 。
流程使用cubemx 创建新项目，使用keil5 调试程序,电机运转，加速减速。
-
创建工程
-
设置时钟
- 设置串口用于输出printf
4. 配置电机引脚
按照要求配置cube 工程
同理根据要求配置电机2
-
配置CCR （比较寄存器的值）的值，控制占空比，，控制输出的平均电压，将周期设置为8000，方便后面的pid 计算
TIM4 配置RCC (电机1)
TIM5 配置RCC(电机2)
控制电机的速度示例代码如下
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
int pwm_value = 1000, temp =0;
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM4_Init();
MX_TIM5_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
//设置电机1的端口输出电平 用于电机2端
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET); //设置为低电平
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_SET); //设置为高电平
//pwm设置来控制电机
//启动pwm
HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3);
//电机2配置
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_RESET); //设置为低电平
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_SET); //设置为高电平
//pwm设置来控制电机
//启动pwm
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
pwm_value = pwm_value + temp;
if(pwm_value >= 8000)
temp = -100;
else
temp = 100;
TIM5->CCR3 = pwm_value;
TIM4->CCR1 = pwm_value;
printf("pwm_value = %d\n",pwm_value);
HAL_Delay(500);
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
上述即电机小demo 测试
2、正交码盘测试小车速度功能开发
正交码盘
原理理解说明
光码盘上有一周的孔，一侧有光敏检测器。另一侧有光源，光源通过孔讲光透过，并会在光敏接受器接收到，每接受一次，产生一个脉冲信号（如上图左侧所示），电动机转动一圈，产生的脉冲信号与孔的数量有关。
即可以得出结论：小车转动一圈产生的脉冲是固定的，控制板接受的脉冲次数可以计算小车的转圈次数，同时可以获取小车速度。
每个电机都有一个编码器，每个编码器输出2路编码器光信号。
编码器测试小demo
-
创建工程
设置时钟，配置串口 略写 -
配置2个电机的编码器的定时器
同理配置TIM2 -
计数周期值得配置RCC
配置成65535 原因
#include <stdio.h>
int main()
{
short i = 65535;
printf("i=%d\n", i);
return 0;
}
结果：-1
分析：因为内存中65535存储内容的16进制表示为：0x0FFFF，将此值传递给16位的变量i时，i只能接受到0xFFFF;看见首位为1，编译器会认为i是个负值，至于负值的绝对值=源码取反（0x0000）+1 = 0x0001。因此最终输出-1。
int main()
{
short i = 65536;
printf("i=%d\n", i);
return 0;
}
结果：0
分析：因为内存中65536存储内容的16进制表示为：0x010000，将此值传递给16的i的时候，i接受到0x0000，编译器认为i=0；
即，获取的值可以根据正负数判断正反转以及转数。
配置编码器进入的IO口的数量，1路或者2路
4. 导出工程项目名
int main(void)
{
/* USER CODE BEGIN 1 */
int encoder_left,encode_right;
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
//启动编码器
HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
//正转是正数，反转是负数
encoder_left = (short)TIM1->CNT; //记录的脉冲的个数.65535
encode_right = (short)TIM2->CNT;
/* USER CODE END WHILE */
TIM1->CNT = 0;
TIM2->CNT = 0;
HAL_Delay(1000);
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
3、小车PID控制功能开发
按照之前的模块，分别把启动电机和启动编码器模块整理在项目中。引入已整理的PID控制文件（下面已发）
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_USART1_UART_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM4_Init();
MX_TIM5_Init();
/* USER CODE BEGIN 2 */
printf("平衡小车开发项目\n");
//启动PWM
HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
//启动编码器
HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
/* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */
MX_FREERTOS_Init();
/* Start scheduler */
osKernelStart();
/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
根据小车的平衡任务分解，我们只是用-直立环PD控制，速度环PI控制，转向环PD
1. 控制车模平衡
直立环的使用 -直立环PD控制，车轮调整方向与倾斜方向许一致，才能保证小车平衡。
倾斜使用的是俯仰角。
/**************************************************************************************************************
*函数名:Vertical_Ring_PD()
*功能:直立环PD控制
*形参:(float Angle):x轴的角度/(float Gyro):x轴的角速度
*返回值:经过PID转换之后的PWM值
**************************************************************************************************************/
//直立环的PD
int Vertical_Ring_PD(float Angle,float Gyro)
{
float Bias;
int balance;
//偏差 Bias
//当前角度 Angle
//调整后的角度 Mechanical_balance (理想状态是平衡的0度)
Bias=Angle-Mechanical_balance; //计算直立偏差
//Balance_Kp
//Balance_Kd
balance=PID.Balance_Kp*Bias+ Gyro*PID.Balance_Kd; //计算直立PWM
return balance; //pwm 值
//printf("balance = %f\n",balance);
}
求值 /Balance_Kp //Balance_Kd
balance=PID.Balance_KpBias+ GyroPID.Balance_Kd;
balance 最大值，为电机最大速度的pwm ,周期值-最大静启动值（电机死区）
例如 我们设置pwm 周期为8000，最大死区为1200，则最大的pwm 为5800
Bias=10，偏移角度，我们设置为10度为最大角度，超过10度小车无法通过移动调节平衡
Balance_Kd = 0
balance=PID.Balance_KpBias+ GyroPID.Balance_Kd;
balance=PID.Balance_Kp10+ Gyro0;
则Balance_Kp = 580 最大值
电机死区
电机的运动，我们同步pwm 不断的增加，从静止到转动，再到加速，从静止到转动有一个摩擦阻止，类似于物理的静摩擦力一样。
2. 控制车模速度-速度环PID调节
/**************************************************************************************************************
*函数名:Vertical_speed_PI()
*功能；速度环PI控制
*形参:(int encoder_left):左轮编码器值/(int encoder_right):编码器右轮的值/(float Angle):x轴角度值
*返回值:
**************************************************************************************************************/
int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement )
{
static float Velocity,Encoder_Least,Encoder;
static float Encoder_Integral;
Encoder_Least =(encoder_left+encoder_right)-0; //获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.8f; //一阶低通滤波器 ，上次的速度占85%
Encoder += Encoder_Least*0.2f; //一阶低通滤波器， 本次的速度占15%
Encoder_Integral +=Encoder; //积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement;
if(Encoder_Integral>10000) Encoder_Integral=10000; //积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //积分限幅
Velocity=Encoder*PID.Velocity_Kp+Encoder_Integral*PID.Velocity_Ki; //速度控制
if(Turn_off(Angle)==1) Encoder_Integral=0; //电机关闭后清除积分
return Velocity;
}
Velocity=EncoderPID.Velocity_Kp+Encoder_IntegralPID.Velocity_Ki; //速度控制
需确定极限值
ki = kp/200
kp 最大值=6000/(160*40%)=93.75
实验需要通过数据的改变来确定我们需要的一个值，
.Velocity_Kp=-？,
.Velocity_Ki=- ？,
当Velocity_Kp 为负值，两轮才能同步方向，维持平衡。
.Velocity_Kp=-52,
.Velocity_Ki=-0.26,
/**************************************************************************************************************
*函数名:Read_Encoder()
*功能:读取编码器值(当作小车当前前进的速度)
*形参:(u8 TIMX):x为编码器1或者2
*返回值:无
*************************************************************************************************************/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 1: Encoder_TIM= (short)TIM1 -> CNT; TIM1 -> CNT=0;break;
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
3. 控制车模方向-转向环PID
2轮平衡车的转向，类似于坦克履带一样，利用速度差进行转向
/**************************************************************************************************************
*函数名:Vertical_turn_PD()
*功能:转向环PD
*形参:无 CCD小于34左转、CCD大于64右转。 yaw = z轴陀螺仪数值
*返回值:无
***************************************************************************************************************/
int Vertical_turn_PD(u8 CCD,short yaw)
{
float Turn;
float Bias;
Bias=CCD-64;
Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
return Turn;
}
Bias 目标角度。比如转向30度
yaw Z轴的角度
CCD 目标角度，正负来确定，转向
Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
Turn=-BiasPID.Turn_Kp-yawPID.Turn_Kd;
Balance_Pwm+Vertical_Pwm-Trun_Pwm = 6000；
平均取中间3000值为最大，
Turn=-BiasPID.Turn_Kp-yawPID.Turn_Kd;
Turn_Kp = 100*Turn_Kd
理想效果就是原地旋转，转向。
4、PID 计算函数（已整理）也可以csdn 上找类似的PID平衡车
功能函数
- 直立环PD控制
- 读取编码器值(当作小车当前前进的速度)
- 速度环PI控制
- 转向环PD
- PWM限幅函数
- 关闭电机
- 输出PWM控制电机
.C文件
#include "math.h"
#include "stdlib.h"
#include "stm32f4xx_hal.h"
#include "contrl.h"
int Dead_Zone=1200; //电机死区 电机静摩擦转动值，达到后，，才会运动，
int control_turn=64; //转向控制
//PID调节参数
struct pid_arg PID = {
.Balance_Kp=200,
.Balance_Kd=1,
.Velocity_Kp=-52,
.Velocity_Ki=-0.26,
.Turn_Kp = 18,
.Turn_Kd = 0.18,
};
/**************************************************************************************************************
*函数名:Read_Encoder()
*功能:读取编码器值(当作小车当前前进的速度)
*形参:(u8 TIMX):x为编码器1或者2
*返回值:无
*************************************************************************************************************/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 1: Encoder_TIM= (short)TIM1 -> CNT; TIM1 -> CNT=0;break;
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
/**************************************************************************************************************
*函数名:Vertical_Ring_PD()
*功能:直立环PD控制
*形参:(float Angle):x轴的角度/(float Gyro):x轴的角速度
*返回值:经过PID转换之后的PWM值
**************************************************************************************************************/
//直立环的PD
int Vertical_Ring_PD(float Angle,float Gyro)
{
float Bias;
int balance;
Bias=Angle-Mechanical_balance;
balance=PID.Balance_Kp*Bias+ Gyro*PID.Balance_Kd;
return balance;
//printf("balance = %f\n",balance);
}
/**************************************************************************************************************
*函数名:Vertical_speed_PI()
*功能；速度环PI控制
*形参:(int encoder_left):左轮编码器值/(int encoder_right):编码器右轮的值/(float Angle):x轴角度值
*返回值:
**************************************************************************************************************/
int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement )
{
static float Velocity,Encoder_Least,Encoder;
static float Encoder_Integral;
Encoder_Least =(encoder_left+encoder_right)-0; //获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.8f; //一阶低通滤波器 ，上次的速度占85%
Encoder += Encoder_Least*0.2f; //一阶低通滤波器， 本次的速度占15%
Encoder_Integral +=Encoder; //积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement;
if(Encoder_Integral>10000) Encoder_Integral=10000; //积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //积分限幅
Velocity=Encoder*PID.Velocity_Kp+Encoder_Integral*PID.Velocity_Ki; //速度控制
if(Turn_off(Angle)==1) Encoder_Integral=0; //电机关闭后清除积分
return Velocity;
}
/**************************************************************************************************************
*函数名:Vertical_turn_PD()
*功能:转向环PD
*形参:无 CCD小于34左转、CCD大于64右转。 yaw = z轴陀螺仪数值
*返回值:无
***************************************************************************************************************/
int Vertical_turn_PD(u8 CCD,short yaw)
{
float Turn;
float Bias;
Bias=CCD-64;
Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
return Turn;
}
/**************************************************************************************************************
*函数名:PWM_Limiting()
*功能:PWM限幅函数
*形参:无
*返回值:无
***************************************************************************************************************/
void PWM_Limiting(int *motor1,int *motor2)
{
int Amplitude=5800;
if(*motor1<-Amplitude) *motor1=-Amplitude;
if(*motor1>Amplitude) *motor1=Amplitude;
if(*motor2<-Amplitude) *motor2=-Amplitude;
if(*motor2>Amplitude) *motor2=Amplitude;
}
/**************************************************************************************************************
*函数名:Turn_off()
*功能:关闭电机
*形参:(const float Angle):x轴角度值
*返回值:1:小车当前处于停止状态/0:小车当前处于正常状态
***************************************************************************************************************/
u8 FS_state;
u8 Turn_off(const float Angle)
{
u8 temp;
if(fabs(Angle)>80){ //当小车角度已经达到80度，我们泽得出小车倒地，关闭电机，
FS_state=1;
temp=1;
AIN2(0), AIN1(0);
BIN1(0), BIN2(0);
}
else
temp=0;
FS_state=0;
return temp;
}
/**************************************************************************************************************
*函数名:Set_PWM()
*功能:输出PWM控制电机
*形参；(int motor1):电机1对应的PWM值/(int motor2):电机2对应的PWM值
*返回值:无
*************************************************************************************************************/
void Set_PWM(int motor1,int motor2)
{
if(motor1>0) AIN2(1), AIN1(0);
else AIN2(0), AIN1(1);
PWMA=Dead_Zone+(abs(motor1))*1.17;
if(motor2>0) BIN1(1), BIN2(0);
else BIN1(0), BIN2(1);
PWMB=Dead_Zone+(abs(motor2))*1.17;
// printf("PWMA = %d\n",PWMA);
// printf("PWMB = %d\n",PWMB);
}
.H文件
#ifndef _CONTRIL_H_
#define _CONTRIL_H_
#include "sys.h"
//机械0点
#define Mechanical_balance 0
#define AIN1(PinState) HAL_GPIO_WritePin( GPIOE, GPIO_PIN_13, (GPIO_PinState)PinState)
#define AIN2(PinState) HAL_GPIO_WritePin( GPIOE, GPIO_PIN_15, (GPIO_PinState)PinState)
#define BIN1(PinState) HAL_GPIO_WritePin( GPIOC, GPIO_PIN_3, (GPIO_PinState)PinState)
#define BIN2(PinState) HAL_GPIO_WritePin( GPIOA, GPIO_PIN_3, (GPIO_PinState)PinState)
#define PWMA TIM4->CCR1
#define PWMB TIM5->CCR3
extern volatile int Encoder_Left,Encoder_Right; //编码器左右速度值
struct pid_arg{
float Balance_Kp;
float Balance_Ki;
float Balance_Kd;
float Velocity_Kp;
float Velocity_Ki;
float Velocity_Kd;
float Turn_Kp;
float Turn_Ki;
float Turn_Kd;
};
extern struct pid_arg PID;
int Read_Encoder(u8 TIMX);
int Vertical_Ring_PD(float Angle,float Gyro);
int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement );
int Vertical_turn_PD(u8 CCD,short yaw);
void PWM_Limiting(int *motor1,int *motor2);
u8 Turn_off(const float Angle);
void Set_PWM(int motor1,int motor2);
#endif
超声波避障小车功能
下一节单独讲解，本次内容过载太长
巡线小车功能
下一节单独讲解，本次内容过载太长
遥控小车
下一节单独讲解，本次内容过载太长
总结
1、理解平衡小车的工作原理，
2、学会使用cubumx 搭建项目，
3、掌握mpu6050 项目工作中使用
4、PID 控制的使用以及原理。
