forked from logzhan/RobotHardware-UESTC
98 lines
3.2 KiB
C
98 lines
3.2 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"
|
||
|
||
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定时
|
||
|
||
|
||
void TIM6_IRQHandler(void)
|
||
{
|
||
// 到达定时器周期
|
||
if(TIM_GetFlagStatus(TIM6, TIM_IT_Update) != RESET)
|
||
{
|
||
TIM_ClearITPendingBit(TIM6, TIM_FLAG_Update); // 清中断
|
||
|
||
LED_Flash(5); // 500ms闪烁一次
|
||
Encoder_Left = Read_Encoder(2) - 30000; // 读取编码器
|
||
Encoder_Right = Read_Encoder(4) - 30000; // 读取编码器
|
||
|
||
//小车的移动速度，即轮子线速度
|
||
//计算车速，左轮和右轮速度和除以2 单位 m/min
|
||
temp3 = (Encoder_Right + Encoder_Left) / 2*600/2496 *0.065*3.1415;
|
||
sprintf(speed,"%2.2f",temp3);
|
||
OLED_ShowString(48,0, (u8*)speed,16); //OLED显示车速
|
||
|
||
if(Encoder_Left < 0)//如果电机反转了
|
||
{
|
||
Encoder_Left = -Encoder_Left;
|
||
}
|
||
if(Encoder_Right < 0)
|
||
{
|
||
Encoder_Right = -Encoder_Right;
|
||
}
|
||
para_L=PID_Calc_Left(Encoder_Left,SetPointL); // 左电机，计数得到增量式PID的增量数值
|
||
para_R=PID_Calc_Right(Encoder_Right,SetPointR); // 右电机，计数得到增量式PID的增量数值
|
||
|
||
if((para_L<-3)||(para_L>3)) // 不做 PID 调整，避免误差较小时频繁调节引起震荡。
|
||
{
|
||
Moto_Left +=para_L;
|
||
}
|
||
if((para_R<-3)||(para_R>3)) // 不做 PID 调整，避免误差较小时频繁调节引起震荡。
|
||
{
|
||
Moto_Right += para_R;
|
||
}
|
||
|
||
if(Moto_Left>3500)Moto_Left=3000; //限幅 防止烧毁电机
|
||
if(Moto_Right>3500)Moto_Right=3000; //限幅
|
||
|
||
Moto_Left=1500;
|
||
Moto_Right=1500;
|
||
TIM3->CCR3=Moto_Left; //更新pwm
|
||
TIM3->CCR4=Moto_Right; //更新pwm
|
||
|
||
//显示pid参数
|
||
showPID();
|
||
//显示左轮和右轮的输出转速 单位RPM
|
||
temp1 = Encoder_Left*600/2496;
|
||
OLED_ShowNum(56,2,temp1,3,12); //左轮实际减速箱输出转速 单位RPM
|
||
temp2 = Encoder_Right*600/2496;
|
||
OLED_ShowNum(56,3,temp2,3,12); //右轮实际减速箱输出转速 单位RPM
|
||
|
||
//设定值显示
|
||
OLED_ShowNum(48,6,SetPoint,3,12); //显示设定的车轮转速 单位RPM
|
||
//temp4 = SetPoint*3.1415*0.065; //计算设定的线速度
|
||
//sprintf(set_speed,"%2.2f",temp4);
|
||
//OLED_ShowString(48,7, set_speed,12); //设定的线速度 单位m/min
|
||
}
|
||
}
|
||
|
||
|
||
|