270 lines
9.3 KiB
C
270 lines
9.3 KiB
C
|
/******************** (C) COPYRIGHT 2023 GeekRebot *****************************
|
|||
|
|
* File Name : car.c
|
|||
|
|
* Current Version : V1.0 & ST 3.5.0
|
|||
|
|
* Author : zhanli 719901725@qq.com
|
|||
|
|
* Date of Issued : 2023.04.06 zhanli: Create
|
|||
|
|
* Comments : GeekRebot 动力调整控制
|
|||
|
|
********************************************************************************/
|
|||
|
|
|
|||
|
|
#include "car.h"
|
|||
|
|
#include "pid.h"
|
|||
|
|
#include "motor.h"
|
|||
|
|
#include "bluetooth.h"
|
|||
|
|
|
|||
|
|
int Encoder_Left, Encoder_Right; // 编码器脉冲数
|
|||
|
|
int Moto_Left, Moto_Right;
|
|||
|
|
int para_L, para_R; // 增量
|
|||
|
|
|
|||
|
|
// 这个设置很重要
|
|||
|
|
int SetPoint = 70; // 设置目标值单位RPM，如果转换成线速度 = setopint*轮子周长 = setpoint*3.14*0.065(轮子直径65mm) 单位就是m/min 一定要搞清楚车轮转速和线速的关系
|
|||
|
|
|
|||
|
|
// 使用减速比是1：120的减速箱。根据实际电机的减速比的配置修改以下参数
|
|||
|
|
// 6240=13*4*120：霍尔编码器13线，STM32编码器模式 4倍频，减速箱1：120
|
|||
|
|
// 2496=13*4*48：霍尔编码器13线，STM32编码器模式 4倍频，减速箱1：48
|
|||
|
|
#define SetPointL SetPoint * 2496 / 600 // 换算成编码器脉冲数，因为最终pid控制的是编码器的脉冲数量
|
|||
|
|
#define SetPointR SetPoint * 2496 / 600 // 换算成编码器脉冲数，因为最终pid控制的是编码器的脉冲数量
|
|||
|
|
|
|||
|
|
// 各种变量
|
|||
|
|
u32 temp1, temp2;
|
|||
|
|
float temp3, temp4;
|
|||
|
|
char set_speed[5]; // 车速显示小数
|
|||
|
|
char speed[5]; // 车速显示小数
|
|||
|
|
// Time1定时器1中断服务函数
|
|||
|
|
// 100ms定时
|
|||
|
|
|
|||
|
|
float Angle_Balance, Gyro_Balance;
|
|||
|
|
u8 Flag_Stop = 1;
|
|||
|
|
|
|||
|
|
static u8 Flag_Target; // 控制函数相关变量，提供10ms基准
|
|||
|
|
int Balance_Pwm, Velocity_Pwm; // 平衡环PWM变量，速度环PWM变量
|
|||
|
|
int Motor_Left, Motor_Right; // 电机PWM变量 应是Motor的 向Moto致敬
|
|||
|
|
|
|||
|
|
void TIM6_IRQHandler(void)
|
|||
|
|
{
|
|||
|
|
// 到达定时器周期
|
|||
|
|
if (TIM_GetFlagStatus(TIM6, TIM_IT_Update) != RESET)
|
|||
|
|
{
|
|||
|
|
TIM_ClearITPendingBit(TIM6, TIM_FLAG_Update); // 清中断
|
|||
|
|
|
|||
|
|
LED_Flash(5); // 500ms闪烁一次
|
|||
|
|
|
|||
|
|
// wheel写法
|
|||
|
|
Encoder_Left = -(Read_Encoder(2) - 30000); // 读取左轮编码器的值，前进为正，后退为负
|
|||
|
|
Encoder_Right = -(Read_Encoder(4) - 30000); // 读取右轮编码器的值，前进为正，后退为负
|
|||
|
|
|
|||
|
|
|
|||
|
|
Get_Angle(); // 更新姿态, 5ms读取一次
|
|||
|
|
|
|||
|
|
// PB_USART_printf(USART2, (u8 *)"Ctl::Pitch = %d\r\n", (int)Pitch);
|
|||
|
|
// PB_USART_printf(USART2, (u8 *)"Encoder Left = %d Right = %d\r\n", (int)Encoder_Left, (int)Encoder_Right);
|
|||
|
|
|
|||
|
|
// // 通过Flag_Target调整控制小车的频率
|
|||
|
|
// if(Flag_Target==1)
|
|||
|
|
// return;
|
|||
|
|
|
|||
|
|
// 平衡PID控制 Gyro_Balance平衡角速度极性：前倾为正，后倾为负
|
|||
|
|
Balance_Pwm = Balance(Angle_Balance, Gyro_Balance);
|
|||
|
|
// 速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
|
|||
|
|
//Velocity_Pwm = Velocity(Encoder_Left,Encoder_Right);
|
|||
|
|
|
|||
|
|
// 如果角度检测到异常, 或者其他需要停止的状态
|
|||
|
|
if (Turn_Off(Angle_Balance))
|
|||
|
|
{
|
|||
|
|
// 停止电机
|
|||
|
|
Car_Stop();
|
|||
|
|
PB_USART_printf(USART2, (u8 *)"Stop\n");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Motor_Left = Balance_Pwm + Velocity_Pwm; // 计算左轮电机最终PWM
|
|||
|
|
Motor_Right = Balance_Pwm + Velocity_Pwm; // 计算右轮电机最终PWM
|
|||
|
|
|
|||
|
|
// PWM限幅
|
|||
|
|
Motor_Left = MotorPwmLimit(Motor_Left, 5500, -5500);
|
|||
|
|
Motor_Right = MotorPwmLimit(Motor_Right, 5500, -5500);
|
|||
|
|
// 设置电机的速度
|
|||
|
|
Motor_SetPwm(Motor_Left, Motor_Right); // 赋值给PWM寄存器
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : Balance
|
|||
|
|
* Description : 直立PD控制
|
|||
|
|
* Input : Angle:角度, Gyro:角速度
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2023/05/20 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
int Balance(float Angle, float Gyro)
|
|||
|
|
{
|
|||
|
|
// 直立环PD参数
|
|||
|
|
float Balance_Kp = 360, Balance_Kd = 0.2;
|
|||
|
|
float Angle_bias, Gyro_bias;
|
|||
|
|
int balance;
|
|||
|
|
// 求出平衡的角度中值 和机械相关
|
|||
|
|
Angle_bias = Middle_angle - Angle;
|
|||
|
|
Gyro_bias = 0 - Gyro;
|
|||
|
|
// 计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
|
|||
|
|
balance = -Balance_Kp * Angle_bias - Gyro_bias * Balance_Kd;
|
|||
|
|
return balance;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: Speed PI control
|
|||
|
|
Input : encoder_left：Left wheel encoder reading；encoder_right：Right wheel encoder reading
|
|||
|
|
Output : Speed control PWM
|
|||
|
|
函数功能：速度控制PWM
|
|||
|
|
入口参数：encoder_left：左轮编码器读数；encoder_right：右轮编码器读数
|
|||
|
|
返回 值：速度控制PWM
|
|||
|
|
**************************************************************************/
|
|||
|
|
int Velocity(int encoder_left, int encoder_right)
|
|||
|
|
{
|
|||
|
|
float Velocity_Kp = 160, Velocity_Ki = 0.8; // 速度环PI参数
|
|||
|
|
static float velocity, Encoder_Least, Encoder_bias;
|
|||
|
|
static float Encoder_Integral;
|
|||
|
|
|
|||
|
|
// 速度PI控制器
|
|||
|
|
// 获取最新速度偏差-测量速度（左右编码器之和）- 目标速度（此处为零）
|
|||
|
|
Encoder_Least = 0 - (encoder_left + encoder_right);
|
|||
|
|
|
|||
|
|
// 一阶低通滤波器
|
|||
|
|
Encoder_bias *= 0.8;
|
|||
|
|
Encoder_bias += Encoder_Least * 0.2;
|
|||
|
|
// 相当于上次偏差的0.8 + 本次偏差的0.2，减缓速度差值，减少对直立的干扰
|
|||
|
|
|
|||
|
|
// 积分出位移 积分时间：10ms
|
|||
|
|
Encoder_Integral += Encoder_bias;
|
|||
|
|
|
|||
|
|
// 积分限幅
|
|||
|
|
if (Encoder_Integral > 10000)
|
|||
|
|
Encoder_Integral = 10000;
|
|||
|
|
if (Encoder_Integral < -10000)
|
|||
|
|
Encoder_Integral = -10000;
|
|||
|
|
|
|||
|
|
// 速度控制
|
|||
|
|
velocity = -Encoder_bias * Velocity_Kp - Encoder_Integral * Velocity_Ki;
|
|||
|
|
if (Turn_Off(Angle_Balance) == 1 || Flag_Stop == 1)
|
|||
|
|
Encoder_Integral = 0; // 电机关闭后清除积分
|
|||
|
|
return velocity;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: Assign to PWM register
|
|||
|
|
Input : motor_left：Left wheel PWM；motor_right：Right wheel PWM
|
|||
|
|
Output : none
|
|||
|
|
函数功能：赋值给PWM寄存器
|
|||
|
|
入口参数：左轮PWM、右轮PWM
|
|||
|
|
返回 值：无
|
|||
|
|
**************************************************************************/
|
|||
|
|
void Motor_SetPwm(int motor_left, int motor_right)
|
|||
|
|
{
|
|||
|
|
if (motor_left > 0)
|
|||
|
|
{
|
|||
|
|
Motor_CtlLeft(GO);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Motor_CtlLeft(BACK);
|
|||
|
|
}
|
|||
|
|
TIM3->CCR3 = myabs(motor_left);
|
|||
|
|
|
|||
|
|
if (motor_right > 0)
|
|||
|
|
{
|
|||
|
|
Motor_CtlRight(GO);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Motor_CtlRight(BACK);
|
|||
|
|
}
|
|||
|
|
TIM3->CCR4 = myabs(motor_right);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: PWM limiting range
|
|||
|
|
Input : IN：Input max：Maximum value min：Minimum value
|
|||
|
|
Output : Output
|
|||
|
|
函数功能：限制PWM赋值
|
|||
|
|
入口参数：IN：输入参数 max：限幅最大值 min：限幅最小值
|
|||
|
|
返回 值：限幅后的值
|
|||
|
|
**************************************************************************/
|
|||
|
|
int MotorPwmLimit(int IN, int max, int min)
|
|||
|
|
{
|
|||
|
|
int OUT = IN;
|
|||
|
|
if (OUT > max)
|
|||
|
|
OUT = max;
|
|||
|
|
if (OUT < min)
|
|||
|
|
OUT = min;
|
|||
|
|
return OUT;
|
|||
|
|
}
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: Press the key to modify the car running state
|
|||
|
|
Input : none
|
|||
|
|
Output : none
|
|||
|
|
函数功能：按键修改小车运行状态
|
|||
|
|
入口参数：无
|
|||
|
|
返回 值：无
|
|||
|
|
**************************************************************************/
|
|||
|
|
void Key(void)
|
|||
|
|
{
|
|||
|
|
u8 tmp;
|
|||
|
|
// tmp=click();
|
|||
|
|
if (tmp == 1)
|
|||
|
|
Flag_Stop = !Flag_Stop; // 单击控制小车的启停
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: If abnormal, turn off the motor
|
|||
|
|
Input : angle：Car inclination；voltage：Voltage
|
|||
|
|
Output : 1：abnormal；0：normal
|
|||
|
|
函数功能：异常关闭电机
|
|||
|
|
入口参数：angle：小车倾角
|
|||
|
|
返回 值：1：异常 0：正常
|
|||
|
|
**************************************************************************/
|
|||
|
|
u8 Turn_Off(float angle)
|
|||
|
|
{
|
|||
|
|
u8 temp;
|
|||
|
|
if (angle < -40 || angle > 40) // 倾角大于40度关闭电机
|
|||
|
|
{ // Flag_Stop置1关闭电机
|
|||
|
|
temp = 1;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
temp = 0;
|
|||
|
|
return temp;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: Get angle
|
|||
|
|
Input : way：The algorithm of getting angle 1：DMP 2：kalman 3：Complementary filtering
|
|||
|
|
Output : none
|
|||
|
|
函数功能：获取角度
|
|||
|
|
入口参数：无
|
|||
|
|
返回 值：无
|
|||
|
|
**************************************************************************/
|
|||
|
|
void Get_Angle(void)
|
|||
|
|
{
|
|||
|
|
Read_DMP(); // 读取加速度、角速度、倾角
|
|||
|
|
Angle_Balance = Pitch; // 更新平衡倾角,前倾为正，后倾为负
|
|||
|
|
Gyro_Balance = gyro[0]; // 更新平衡角速度,前倾为正，后倾为负
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**************************************************************************
|
|||
|
|
Function: Absolute value function
|
|||
|
|
Input : a：Number to be converted
|
|||
|
|
Output : unsigned int
|
|||
|
|
函数功能：绝对值函数
|
|||
|
|
入口参数：a：需要计算绝对值的数
|
|||
|
|
返回 值：无符号整型
|
|||
|
|
**************************************************************************/
|
|||
|
|
int myabs(int a)
|
|||
|
|
{
|
|||
|
|
int temp;
|
|||
|
|
if (a < 0)
|
|||
|
|
{
|
|||
|
|
temp = -a;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
temp = a;
|
|||
|
|
}
|
|||
|
|
return temp;
|
|||
|
|
}
|