代码收藏家 基于STM32的平衡车控制与运动实现（HAL库）
主控为stm32的平衡车，以及平衡车运动（HAL库）
一些放在前面的碎碎念，一开始上手stm32用的是库函数，后来因为电赛原因，怕 f103c8t6的定时器不够用，开始学习使用hal库。不得不说，真的很方便，用了之后，都不太愿意用库函数了。
这次这篇，是边做平衡车边写博客，因为怕自己做完一个东西之后，就懒得复盘写博客了（就算勤快起来，也怕后面会忘了一些比较重要的东西没写进博客里面，做一个东西总会遇到一些神奇的错误，然后后续还会遇到，但后续会忘了自己之前怎么解决的，所以写一篇博客是目前来看最好的复盘）
完整代码已经上传至我个人的github账号上了
硬件部分
pcb图
整个工程完善后，会将pcb开源
上面这个是我的底板
编码电机
编码电机这边比较烧脑的就是接线（说起来有点丢人，我之前刚开始接触编码电机时，卡接线卡了一段时间，然后翻找博客半天才懂怎么使用编码电机）
这张图是商家给出的说明（可能不同电机不一样的接线，最好看下自己的电机是什么型号的，再对应图接线）
然后是里面的1，6线，这个比较简单，类比那种黄色减速小电机上面的两根线，如果不需要测速只需要驱动电机的话，可以只接这两根线，用法和黄色减速小电机一样。
当然，用编码电机大概率会用到编码器测速的。我用的是左边这款霍尔编码器款，其中的2编码器电源是接VCC，也就是3.3V，而5编码器地线是接GND，这个都比较显然。然后是3，4线，这个就是接定时器的通道一和通道二，比如我用的是TIM3和TIM4给两个电机测速，TIM3这边和电机接线的就是A6与A7，TIM4这边就是B6与B7。
CubeMX配置
这个就是按照步骤来，比较简单。
Pinout & Configuration
配置System Core
将 High Speed Clock(HSE) 与 Low Speed Clock (LSE) 配置为Crystal/Ceramic Resonator
将Debug配置为Serial Wire，用于stlink 烧录程序
配置Timers
f103c8t6中的定时器资源有：高级定时器TIM1，通用定时器TIM2、TIM3、TIM4
这里我是把TIM1用于计时，TIM2用于输出PWM，TIM3与TIM4用于编码器测速（测脉冲）
配置TIM1（定时0.1s）
Clock Source 选择 Internal Clock，PSC为719，Counter Period为9999。配置为0.1S定时中断一次读取TIM3、TIM4编码器模式记录的脉冲值
配置TIM2（输出占空比PWM）
Clock Source 选择 Internal Clock，我用的是通道1和通道2，所以Channel1选择PWM Generation CH1， Channel2选择PWM Generation CH2，然后Counter Period设置为7199。
配置TIM3、TIM4（编码器测速）
Combined Channels选择Encoder Mode，然后下面的Encoder Mode选择为Encoder Mode TI1 and TI2，使得编码器模式为4倍频模式，得到的脉冲值X4
配置Connectivity
配置I2C
上面的I2C选择I2C，下面的I2C Speed Mode 选择Fast Mode，这个是mpu6050需要的通信速度
配置USART1
这个USART1是用来打印输出数据的，串口输出数据，在上位机上显示。Mode选择Asynchronous。
配置电机控制引脚
也就是控制电机正反转的 IO口，我选择了A4、A5驱动TIM3测速的电机，B0、B1驱动TIM4测速的电机（这几根线，同样类比驱动黄色减速小电机，这样就比较好理解了），配置的时候，我在User Label这边给引脚重新命名，方便代码移植。
配置中断优先级
System view -> NVIC -> TIM1 update interrupt
Clock Configuration
配置时钟树，在HCLK（MHz）里面输入最大值72，然后CubeMX会自动配置好时钟树。
Project Manager
keil代码
printf重定向
在main.c先添加上#include < stdio.h>
然后
Options for Target这里，要记得勾选上Use Micro LIB，不然用不了printf
按照上面配置好后，就可以像C语言一样，使用printf去打印数据，数据可以传到电脑上位机去观看了，比较方便。
比如像下面这样去使用，打印encoder_countera，与encoder_counterb上传到上位机观测。
printf("%d,%d\r\n",encoder_countera, encoder_counterb);
定时器
驱动电机
motor.c代码
#include "motor.h"
#include "tim.h"
#include "main.h"
/**
* @function:void set_motor1(int16_t pwm)
* @description: 控制电机转速
* @param {pwm，范围在-7200到7200}
* @return {*}
*/
void set_motor1(int16_t pwm)
{
if(pwm >= 0)
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1,pwm);
HAL_GPIO_WritePin (AIN1_GPIO_Port ,AIN1_Pin ,GPIO_PIN_RESET );
HAL_GPIO_WritePin (AIN2_GPIO_Port ,AIN2_Pin ,GPIO_PIN_SET );
}
else
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1,-pwm);
HAL_GPIO_WritePin (AIN1_GPIO_Port ,AIN1_Pin ,GPIO_PIN_SET );
HAL_GPIO_WritePin (AIN2_GPIO_Port ,AIN2_Pin ,GPIO_PIN_RESET );
}
}
void set_motor2(int16_t pwm)
{
if(pwm >= 0)
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2,pwm);
HAL_GPIO_WritePin (BIN1_GPIO_Port ,BIN1_Pin ,GPIO_PIN_RESET );
HAL_GPIO_WritePin (BIN2_GPIO_Port ,BIN2_Pin ,GPIO_PIN_SET );
}
else
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2,-pwm);
HAL_GPIO_WritePin (BIN1_GPIO_Port ,BIN1_Pin ,GPIO_PIN_SET );
HAL_GPIO_WritePin (BIN2_GPIO_Port ,BIN2_Pin ,GPIO_PIN_RESET );
}
}
motor.h代码
#ifndef MOTOR_MOTOR_H_
#define MOTOR_MOTOR_H_
#include "stm32f1xx_hal.h" //HAL库文件声明
#include <main.h>
void set_motor1(int16_t pwm);
void set_motor2(int16_t pwm);
#endif
电机测速
encoder.c代码
#include "encoder.h"
#include "main.h"
#include "usart.h"
#include "tim.h"
#define PI 3.1415
float na = 0;//转速，单位为：转/秒
float nb = 0;
double speeda = 0;//转速，单位为：m/s
double speedb = 0;
short encoder_countera = 0;//STM32编码器模式读取的总脉冲数
short encoder_counterb = 0;
/**
* @function: short GET_NUMA(void)
* @description: 使用STM32编码器模式，读取编码器产生的脉冲值
* @param {*}
* @return {encoder_countera}
*/
short GET_NUMA(void)
{
return encoder_countera;
}
short GET_NUMB(void)
{
return encoder_counterb;
}
/**
* @function:void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
* @description: 定时器中断回调函数，0.1S中断一次并计算转速，将电机转速通过USART1传回上位机
* @param {TIM_HandleTypeDef *htim}
* @return {*}
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim==&htim1)
{
encoder_countera = (short) __HAL_TIM_GET_COUNTER(&htim3);//使用STM32编码器模式，读取编码器产生的脉冲值
__HAL_TIM_SET_COUNTER(&htim3,0);
encoder_counterb = (short) __HAL_TIM_GET_COUNTER(&htim4);
__HAL_TIM_SET_COUNTER(&htim4,0);
na=(float)encoder_countera/1040/0.1;
nb=(float)encoder_counterb/1040/0.1;
//使用的电机：30减速比13线编码器，所以车轮转一圈是输出260个脉冲，四倍频是1040。
speeda = na * 0.048 * PI;
speedb = nb * 0.048 * PI;
printf("%lf,%lf\r\n",speeda, speedb);
}
}
这里有一个小细节，就是如果想要打印换行，需要的是\r\n，如果只有\n的话，上位机显示不出换行。
encoder.h代码
#ifndef ENCODER_ENCODER_H_
#define ENCODER_ENCODER_H_
#include "stm32f1xx_hal.h" //HAL库文件声明
#include <main.h>
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;
short GET_NUMA(void);
short GET_NUMB(void);
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim);
#endif /* ENCODER_ENCODER_H_ */
mpu6050角度传感器
这个的移植方法，我有专门发一篇的博客去写，可以直接看我那一篇的博客去跟着配置，很简单的，至于mpu6050我配置好的代码，我已经发到了我个人的github账号上。
就是跟着我这篇博客来，看完你就会比较明白了。
主函数
平衡车原理
其实这个我只会一笔带过，因为我这篇博客主要是写这些要配置的代码怎么敲（因为我看CSDN上写平衡车原理的有很多，至于cubemx怎么配置，keil中代码怎么写的，没什么人写）
平衡车的设计目标是保证小车能够在启动后快速到达平衡位置，并且能够在外界施加随机扰动后依旧没有震荡和过大的角度偏移，即小车达到期望的状态后，能够通过控制系统实现动态稳定。
pid算法：
其实我个人并不认为平衡车的pid算法是串级pid，我觉得这个跟串级pid的原理不一样。
串级pid：比如说第一个环输入的是电流量，经过第一个pid环，输出的是电机的速度量，再把电机的速度量输入第二个pid环，经过第二个pid环，输出电机的位置量，这叫串级pid。
而平衡车的pid，只是将PD直立环（balance），PI速度环（velocity）和PD转向环（turn）的结果分别计算然后相加，所以我不觉得这个算串级pid。
最后言归正传，来说说平衡车里的pid环。平衡车里的pid环，将PD直立环（balance），PI速度环（velocity）和PD转向环（turn）的结果分别计算然后相加，得到输出左右轮的pwm_a和pwm_b输出。
pwm_a = balance + velocity – turn
pwm_b = balance + velocity + turn
其中直立环使用比例+微分变量（PD）进行控制，比例系数计算小车倾角与平衡位置的差(error)，而微分系数计算小车俯仰角的加速度(gyroy)。小车的直立情况和积分量无关，主要与其位置和加速度有关，具体计算公式如下：
balance = pid_balance.kp * (pitch – pid_angle) + gy * pid_balance .kd;
速度环使用优化后的限幅比例+积分比例（PI）进行控制，比例系数计算小车左右轮速度之和和零之间的差值(Encoder)，积分系数计算小车速度差累加和(Encoder_Integral)，由于速度控制与小车加速度几乎无关，所以只是用PI控制，具体公式如下：
velocity = Encoder * pid_velocity.kp + Encoder_Integral * pid_velocity.ki;
Encoder = Encoder * 0.7 + (encoder_left + encoder_right) * 0.3;
Encoder_Integral += Encoder;
转向环一般可以单独使用PI控制，直接差速即可，也可以使用限幅的PD控制，通过轮胎的反馈速度和航向角Yaw来给定差速。比例系数计算控制理想差速度（设置为0，小车理想状态是处于静止平衡状态）和实际差速度的差值 (encoder_left – encoder_right)，微分系数计算z轴陀螺仪转向角（yaw），具体计算公式如下：
turn = (encoder_left – encoder_right) * pid_turn.kp – yaw * pid_turn.kd;
将上面三个融合在一起，就可以得到平衡车需要的pid算法了。
double speed_pid(int actual_speed, int want_speed);
float pitch;
float roll;
float yaw;
struct pid
{
double kp;
double ki;
double kd;
};
double balance = 0;//直立环得到的值
struct pid pid_balance = {
.kp = 246,//246,
.kd = 0.6//0.6
};
double velocity = 0;//速度环得到的值
double Encoder = 0;
double encoder_left = 0;
double encoder_right = 0;
double Encoder_Integral = 0;
struct pid pid_velocity = {
.kp = 4,//7
.ki = 0.02//0.035
};
double turn = 0;//转向环得到的值
double turn_rl = 0;//控制车转弯
struct pid pid_turn = {
.kp = -3,
.kd = 0
};
double pid_angle = 0.1;//车平衡时候的角度
#define PID_ANGLE 0.1车身保持平衡时候的角度值
double pwm_a = 0;//加给左轮的pwm值
double pwm_b = 0;//加给右轮的pwm值
void PwmPid(double* pwm_a, double* pwm_b);//声明pid的算法
/*
* 输出pwm的pid
*/
void PwmPid(double* pwm_a, double* pwm_b)
{
//下面第一行和第三行都是为了获得目前的车身角度值
MPU6050_Get();
HAL_UART_Receive_IT (&huart1,&temp,1);
if(mpu_dmp_get_data(&pitch ,&roll,&yaw) == 0){}
//下面这行为：直立环的计算
balance = pid_balance.kp * (pitch - pid_angle) + gy * pid_balance .kd;
encoder_left = GET_NUMA ();//用于得到左轮的速度
encoder_right = GET_NUMB();//用于得到右轮的速度
/*下面三行为速度环的计算*/
velocity = Encoder * pid_velocity.kp + Encoder_Integral * pid_velocity.ki;
Encoder = Encoder * 0.7 + (encoder_left + encoder_right) * 0.3;
Encoder_Integral += Encoder;
/*防止积分过大而限幅*/
if(Encoder_Integral > 10000)
Encoder_Integral = 10000;
if(Encoder_Integral < -10000)
Encoder_Integral = -10000;
//下面这行为转向环的计算
turn = (encoder_left - encoder_right) * pid_turn.kp - yaw * pid_turn.kd;
*pwm_a = balance + velocity + turn;
*pwm_b = balance + velocity - turn;
}
HAL_UART_Receive_IT (&huart1,&temp,1);
心细的应该可以看到上述代码中有这行，其实这行是我做遥控小车才加上的，如果你不做遥控小车，就不需要管这行，这行的代码意思是，接收usrt1的信息。
至此，平衡车的主要代码就讲解完了。
平衡车难以保持平衡，一直向前向后摇摆：电机死区
明明平衡车参数已经调了很久，但是平衡车还是达不到静如处子的状态，这是电机死区的问题。
什么是电机死区呢，这么说把，我给电机占空比20%的时候，电机可以转动，但是我给电机占空比10%的时候，电机却不能转动，那按照理论上来说，给10%的占空比，电机应该缓慢转动的，但是此时电机却不能转动，那么这就是电机的死区。
怎么解决呢，这个很简单，就是在我们电机的motor.c里面改。
不是说给10%不能转动吗，那么我们就找，给多少时，这个电机开始转动，那么这个值就是临界值，我们可以把这个值当作0值。感觉这么说还是有点抽象，这么说吧。本来我在我的set_motor1(0)这个函数里面，我传值为0，那么此时我输入的就是0%的占空比，但是我把我的代码改成下面这样，
我加上了两句：pwm = pwm + 440；pwm = pwm – 440
那么此时我的set_motor1(0)传入值0，其实我给电机的占空比已经不是0了，而是440/7200 = 6.1%，而此时我传入值0，电机处于一种即将开始转动的状态。那么这样电机死区问题就解决了。
平衡车的运动问题
只会平衡的平衡车不是好的平衡车，要会动的平衡车才是个好的平衡车，那么怎么让平衡车动如脱兔呢。
平衡车有两种运动形态，一种向前向后的直线运动，一种左转右转的转向运动。
向前向后的直线运动，我们可以更改平衡车平衡时的角度值，比如平衡车平衡时是0.1度，那么我们更改这个值，告诉平衡车是3度，那么平衡车就会去追逐这个3度，达到向前运动的状态，同理，向后运动改成-2.8度。
而左转右转的转向运动，我又引入了一个变量叫turn_rl这个变量。把这个变量的值加入到pwm_a 与pwm_b中，如下，我就可以达到转向的目的。
*pwm_a = balance + velocity + turn + turn_rl;
*pwm_b = balance + velocity - turn - turn_rl;
比如我想要左转的时候，只需要给turn_rl赋值300，此时两边轮子的占空比数差了600/7200，两边轮子产生差速，就可以转向了。而想要右转的时候，只需要给turn_rl赋值-300，此时就可以右转了。
主函数完整代码
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "encoder.h"
#include "motor.h"
#include "DMP_test.h"
#include "mpu6050.h"
void DMP_test(void);
double speed_pid(int actual_speed, int want_speed);
float pitch;
float roll;
float yaw;
struct pid
{
double kp;
double ki;
double kd;
};
double balance = 0;//直立环得到的值
struct pid pid_balance = {
.kp = 246,//246,
.kd = 0.6//0.6
};
double velocity = 0;//速度环得到的值
double Encoder = 0;
double encoder_left = 0;
double encoder_right = 0;
double Encoder_Integral = 0;
struct pid pid_velocity = {
.kp = 4,//7
.ki = 0.02//0.035
};
double turn = 0;//转向环得到的值
double turn_rl = 0;//控制车转弯
struct pid pid_turn = {
.kp = -3,
.kd = 0
};
double pid_angle = 0.1;//车平衡时候的角度
#define PID_ANGLE 0.1
double pwm_a = 0;//加给左轮的pwm值
double pwm_b = 0;//加给右轮的pwm值
uint8_t temp;
uint8_t message = 0;//判断平衡车的运动状态
uint8_t transfer[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int fputc(int ch, FILE *f){
uint8_t temp[1] = {ch};
HAL_UART_Transmit(&huart2,temp,1,2);
return ch;
}
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
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_DMA_Init();
MX_I2C1_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_USART1_UART_Init();
MX_USART2_UART_Init();
/* USER CODE BEGIN 2 */
DMP_test();
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); //开启TIM2 PWM模式
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); //开启TIM2 PWM模式
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);//开启TIM3编码器模式
HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);//开启TIM4编码器模式
HAL_TIM_Base_Start_IT(&htim1);//开启TIM1的定时器中断
void PwmPid(double* pwm_a, double* pwm_b);
uint8_t flag = 0;
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
PwmPid(&pwm_a, &pwm_b);
set_motor1(pwm_a);
set_motor2(pwm_b);
HAL_UART_DMAResume(&huart1);
HAL_UART_Receive_DMA(&huart1, (uint8_t *)transfer, sizeof(transfer));
if(transfer[0] == 0)
{}
if(transfer[1] == 1)
{
flag = 1;
HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
pid_angle = PID_ANGLE + 3;
pid_turn.kp = -5.5;
turn_rl = 0;
HAL_Delay (300);
}
if(transfer[2] == 2)
{
HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
pid_angle = PID_ANGLE - 3;
pid_turn.kp = -5.5;
turn_rl = 0;
HAL_Delay (300);
}
if(transfer[3] == 3)
{
HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
pid_angle = PID_ANGLE;
pid_turn.kp = 0;
turn_rl = 300;
HAL_Delay (300);
}
if(transfer[4] == 4)
{
HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
pid_angle = PID_ANGLE;
pid_turn.kp = 0;
turn_rl = -300;
HAL_Delay (300);
}
if(transfer[5] == 5)
{}
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
/*
* 输出pwm的pid
*/
void PwmPid(double* pwm_a, double* pwm_b)
{
MPU6050_Get();
HAL_UART_Receive_IT (&huart1,&temp,1);
if(mpu_dmp_get_data(&pitch ,&roll,&yaw) == 0){}
balance = pid_balance.kp * (pitch - pid_angle) + gy * pid_balance .kd;
encoder_left = GET_NUMA ();
encoder_right = GET_NUMB();
velocity = Encoder * pid_velocity.kp + Encoder_Integral * pid_velocity.ki;
Encoder = Encoder * 0.7 + (encoder_left + encoder_right) * 0.3;
Encoder_Integral += Encoder;
if(Encoder_Integral > 10000)
Encoder_Integral = 10000;
if(Encoder_Integral < -10000)
Encoder_Integral = -10000;
turn = (encoder_left - encoder_right) * pid_turn.kp - yaw * pid_turn.kd;
*pwm_a = balance + velocity + turn + turn_rl;
*pwm_b = balance + velocity - turn - turn_rl;
}
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
//{
// if(huart->Instance==USART1)
// {
// if(transfer[1] == 3)
// {
// HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
// pid_angle = PID_ANGLE + 3;
// pid_turn.kp = -5.5;
// turn_rl = 0;
// }
// if(transfer[2] == 3)
// {
// HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
// pid_angle = PID_ANGLE - 3;
// pid_turn.kp = -5.5;
// turn_rl = 0;
// }
// if(transfer[3] == 3)
// {
// HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
// pid_angle = PID_ANGLE;
// pid_turn.kp = 0;
// turn_rl = 300;
// }
// if(transfer[4] == 3)
// {
// HAL_GPIO_WritePin (GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
// pid_angle = PID_ANGLE;
// pid_turn.kp = 0;
// turn_rl = -300;
// }
// //HAL_UART_Receive_IT(&huart1,&temp,1);
// }
//}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
