全国大学生智能汽车大赛（二）：电感采样、卡尔曼滤波、方向控制代码
全国大学生智能汽车大赛（四）：电机控制代码及主函数
🥳🥸下面为正文🥰👺
一、电机控制代码
/*******************************
* 电机控制
* 编码器
*作者：躺躺我啊
*最终修改日期：2021.6.20
*TC264D
*******************************/
#include "headfile.h"
speed_direction speed_param;
short last_pid_speed=0; //上次的编码器获得得值
//非特殊元素上跑的速度
float SET=300;
float KP =14.5;
float KI =0.3;
float KD =0.3;
//向上位机发送信息用
int speed_now_int,speed_out_int;
//pid参数初始化
void speed_pid_init(void)
{
speed_param.kp = KP; //P
speed_param.ki = KI; //I
speed_param.kd = KD; //D
speed_param.set = 100; //目标速度
speed_param.speed_standard = 100; //速度标准
speed_param.now = 0; //当前速度
speed_param.last = 0; //上次速度
speed_param.imax = 100; //积分限幅
speed_param.pout = 0; //KP输出
speed_param.iout = 0; //KI输出
speed_param.dout = 0; //KD输出
speed_param.out = 0; //PID输出
speed_param.isum = 0; //积分值
speed_param.error[0] = 0; //本次误差
speed_param.error[1] = 0; //上次误差
speed_param.error[2] = 0; //上上次误差
speed_param.last_t = 0; //上次时间
speed_param.counts_flag = 0; //计数标志
speed_param.counts = 0; //计数
speed_param.speed_real = 0; //实际速度
speed_param.distance = 0; //距离,单位是米
}
/************************************************
* 功能说明：电机初始化
* 修改时间：2021年5月19日
*************************************************/
void motor_init (void)
{
//电机初始化
gtm_pwm_init(ATOM0_CH4_P02_4,14*1000,0);
gpio_init(P02_4,GPO,0,NO_PULL);
gtm_pwm_init(ATOM0_CH5_P02_5,14*1000,0);
systick_delay_ms(STM0,40);
//电机控制
pwm_duty(ATOM0_CH4_P02_4 , 0);
gpio_set(P02_4,1);
pwm_duty(ATOM0_CH5_P02_5 , speed_param.speed_standard*20);
}
/*********************************************
* 功能说明：计算实际速度及距离
* 参数说明：calculate
* 修改时间：2021年6月17日
**********************************************/
void calculate(void)
{
if(speed_param.counts_flag == 1)
{
speed_param.counts += speed_param.now;
speed_param.distance = speed_param.counts * 0.0001718;//使用编码器总计数来计算实际距离
}
}
//float ser=200;
//speed_error bangbang算法所定目标阈值 小于或大于此值调用bangbang
/*********************************************
* 功能说明：速度控制
* 参数说明：velocity
* 修改时间：2021年6月15日
**********************************************/
void pwm_change(int velocity)
{
// if(velocity <= -2000) velocity=-2000;
// if(velocity >= 2000) velocity=2000;
if(velocity >= 0)
{
pwm_duty(ATOM0_CH4_P02_4,0);
pwm_duty(ATOM0_CH5_P02_5,velocity);
}
else if(velocity<0)
{
pwm_duty(ATOM0_CH4_P02_4,velocity);
pwm_duty(ATOM0_CH5_P02_5,0);
}
}
/*********************************************************************************
* 功能说明：电机控制
* 修改时间：2021年6月15日
* 备注：当前编码器采集周期为8000us，车轮直径56mm,转一圈车向前行驶0.176m，换算下来，要跑2m/s的话，编码器输出大致为93
**********************************************************************************/
void motor_control(void)
{
/****************************************采样计算****************************************/
speed_param.now = gpt12_get(GPT12_T5); //测试得负号为正值??
gpt12_clear(GPT12_T5); //清除编码器计数
if(speed_param.now > 250) //用上位机可以看到 有个尖端脉冲,毛刺 直接用这个函数滤掉, 过滤阈值依自己得速度来定
{
speed_param.now = speed_param.last;
}
speed_param.last = speed_param.now;
//speed_param.speed_real = (speed_param.now * 56 * 3.1415926*0.001) / (0.008*1024);
speed_param.speed_real = speed_param.now * 0.0215; //由上式换算得出当前实际速度
calculate(); //计算实际速度及距离
/****************************************控制****************************************/
pid_incremental(); //增量式PID
if (speed_param.out > 4000) speed_param.out = 4000; //限幅
else if(speed_param.out<-4000) speed_param.out =-4000; //限幅
//pwm_change(speed_param.out); //速度控制
speed_now_int = speed_param.now; //将当前速度转换为int，发送至上位机
speed_out_int = speed_param.out; //将pid输出转换为int，发送至上位机
}
//PID增量式控制器输出
void pid_incremental(void)
{
speed_param.error[0] = speed_param.set - speed_param.now;
speed_param.pout = speed_param.kp * (speed_param.error[0] - speed_param.error[1]);
speed_param.iout = speed_param.ki * speed_param.error[0];
speed_param.dout = speed_param.kd * (speed_param.error[0] - 2*speed_param.error[1] + speed_param.error[2]);
speed_param.error[2] = speed_param.error[1];
speed_param.error[1] = speed_param.error[0];
speed_param.out += speed_param.pout + speed_param.iout + speed_param.dout;
}
二、主函数
void init_all(void)
{
//角度环初始化
direction_init();
//速度环初始化
speed_pid_init();
//初始化蜂鸣器引脚
gpio_init(P33_10, GPO, 0, PUSHPULL);
//设置P20_8为输出 默认输出低电平 PUSHPULL：推挽输出
gpio_init(P20_8, GPO, 0, PUSHPULL);
//编码器初始化
gpt12_init(GPT12_T5,GPT12_T5INB_P10_3,GPT12_T5EUDB_P10_1);
//摄像头初始化
mt9v03x_init();
//舵机初始化
servo_init();
//电感初始化
inductor_init();
//电机初始化
motor_init();
//串口通信初始化
wireless_init();
data_init();
//使用GPT12 定时器T2 P33_7为计数引脚 P33_6为方向引脚
gpt12_init(GPT12_T5, GPT12_T5INB_P10_3, GPT12_T5EUDB_P10_1);
//卡尔曼滤波变量初始化
kfp_init();
//速度定时器初始化
pit_init(CCU6_0,PIT_CH0,8000); //改这个周期时间的同时需要更改speed.c文件中速度换算的公式
//控制定时器初始化
pit_init(CCU6_0,PIT_CH1,4000);
}
//一闪一闪亮晶晶
void blink_blink(void)
{
gpio_toggle(P20_8); //翻转IO
systick_delay_ms(STM0, 5); //延时
}
int core0_main(void)
{
get_clk(); //获取时钟频率 务必保留
init_all();
enableInterrupts();
//2.0寸 IPS液晶初始化
ips200_init();
//等待所有核心初始化完毕
IfxCpu_emitEvent(&g_cpuSyncEvent);
IfxCpu_waitEvent(&g_cpuSyncEvent, 0xFFFF);
while (TRUE)
{
show_all(); //显示
blink_blink(); //LED闪烁
send_to_pc(); //上位机通信
systick_delay_ms(STM0, 100); //延时
}
}
三、 推荐IO分配
摄像头：8个数据口，一个串口，两eru中断
数据口：00_0 00_1 00_2 00_3 00_4 00_5 00_6 00_7
配置串口：摄像头RX：02_2 摄像头TX：02_3
VSY：02_0
HREF：程序不需要，所以不接
PCLK：02_1
四路运放 A0 A1 A2 A3 等
四个编码器：
LSB：33_7 DIR：33_6
LSB：02_8 DIR：00_9
LSB：10_3 DIR：10_1
LSB：20_3 DIR：20_0
8路pwm：
21_2 21_3 21_4 21_5 02_4 02_5 02_6 02_7
ICM20602：
CLK： P20_11
MOSI： P20_14
MISO： P20_12
CS： P20_13
SPI0
TFT屏幕：
CLK 15_3
MOSI 15_5
MISO 15_4 //实际上TFT没有这个引脚 这里仅仅占位而已
CS0 15_2
BL 15_4 //复用控制背光
REST 15_1
DC 15_0
SPI2
舵机：P33_9
尽量不要使用以下引脚，以下引脚属于boot引脚，不合理的使用容易导致单片机无法启动等问题，因此建议大家尽量不要使用：
P14_2
P14_3
P14_4
P14_5
P14_6
P10_5
P10_6
扫一扫
487
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
