当前位置：物联沃-IOTWORD物联网 > 技术教程 > 位机软件编写STM32F407平衡车项目：上位机软件编写详解
代码收藏家代码收藏家 技术教程

位机软件编写STM32F407平衡车项目：上位机软件编写详解

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平衡车
  • 超声波避障小车功能
  • 巡线小车功能
  • 遥控小车
  • 总结

    • 前言

    提示：这里可以添加本文要记录的大概内容：

    例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。

    一、平衡小车

    实物图如下所示各类平衡车

    平衡小车的功能介绍

    大容量电池供电
    双马达电机驱动
    平衡控制调节
    速度控制调节
    方向控制条件
    上位机远程控制

    平衡小车功能开发需求

    1. 平衡小车PID控制
      直立、速度、方向
    2. 平衡小车遥控功能
      无线通信功能、上位机App
    3. 平衡小车避障功能
      超声波测距
    4. 平衡小车巡线功能（线路轨迹运行）
      摄像头CCD循迹
    5. 菜单显示及功能设置
      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库使用

    1. 初始化MPU6050
    2. 移植MPU6050官方的eMPL库
    3. 获取原始数据以及姿态

    注意 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 调试程序,电机运转，加速减速。

    1. 创建工程

    2. 设置时钟

    1. 设置串口用于输出printf


    4. 配置电机引脚


    按照要求配置cube 工程


    同理根据要求配置电机2

    1. 配置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

    1. 创建工程

      设置时钟，配置串口 略写

    2. 配置2个电机的编码器的定时器

      同理配置TIM2

    3. 计数周期值得配置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 控制的使用以及原理。

    下一节，stm32项目平衡车详解(stm32F407)下

    您可能感兴趣的内容:

    物联沃分享整理
    物联沃-IOTWORD物联网 » 位机软件编写STM32F407平衡车项目：上位机软件编写详解

    代码收藏家 普通

    分享到：

    相关推荐

    发表评论