You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
129 lines
2.6 KiB
129 lines
2.6 KiB
3 years ago
|
/*
|
||
|
|
* @Author: your name
|
||
|
|
* @Date: 2022-04-17 21:03:48
|
||
|
|
* @LastEditTime: 2022-04-18 00:11:26
|
||
|
|
* @LastEditors: Please set LastEditors
|
||
|
|
* @Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||
|
|
* @FilePath: \undefinede:\项目\兼职项目\MotorPid\stm32\USER\main.c
|
||
|
|
*/
|
||
|
|
#include "led.h"
|
||
|
|
#include "delay.h"
|
||
|
|
#include "sys.h"
|
||
|
|
#include "usart.h"
|
||
|
|
#include "wifi.h"
|
||
|
|
#include "adc.h"
|
||
|
|
#include "timer.h"
|
||
|
|
#include "string.h"
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <string.h>
|
||
|
|
#include "oled.h"
|
||
|
|
#include "motor.h"
|
||
|
|
#include "encoder.h"
|
||
|
|
#include "key.h"
|
||
|
|
|
||
|
|
struct UserInfo userInfo;
|
||
|
|
|
||
|
|
// 函数声明
|
||
|
|
void Sys_Init(void);
|
||
|
|
void processingTasks(void);
|
||
|
|
void OLED_Display(void);
|
||
|
|
|
||
|
|
float Velocity_KP_Mode2=50,Velocity_KI_Mode2=10;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @description: main函数
|
||
|
|
* @param {*}
|
||
|
|
* @return {*}
|
||
|
|
*/
|
||
|
|
int main(void)
|
||
|
|
{
|
||
|
|
Sys_Init();
|
||
|
|
delay_ms(1000);
|
||
|
|
while(1)
|
||
|
|
{
|
||
|
|
processingTasks();
|
||
|
|
OLED_Display();
|
||
|
|
delay_ms(100);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @description: pid 算法
|
||
|
|
* @param {int} targetSpeed
|
||
|
|
* @param {int} currentSpeed
|
||
|
|
* @return {*}
|
||
|
|
*/
|
||
|
|
int velocity_PID(int targetSpeed, int currentSpeed){
|
||
|
|
static float velocity_Kp = 0.04, velocity_Kd = 0;
|
||
|
|
int Bias, velocity = 0;
|
||
|
|
Bias = currentSpeed - targetSpeed;
|
||
|
|
velocity = velocity_Kp * Bias + Bias * velocity_Kd;
|
||
|
|
return velocity;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int Incremental_PI_B_Mode2 (int Encoder,int Target)
|
||
|
|
{
|
||
|
|
static int Bias,Pwm,Last_bias;
|
||
|
|
Bias=Encoder-Target; //计算偏差
|
||
|
|
Pwm+=Velocity_KP_Mode2*(Bias-Last_bias)+Velocity_KI_Mode2*Bias; //增量式PI控制器
|
||
|
|
if(Pwm>7200)Pwm=7200;
|
||
|
|
if(Pwm<-7200)Pwm=-7200;
|
||
|
|
Last_bias=Bias; //保存上一次偏差
|
||
|
|
return Pwm; //增量输出
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @description: 处理任务
|
||
|
|
* @param {*}
|
||
|
|
* @return {*}
|
||
|
|
*/
|
||
|
|
void processingTasks(void)
|
||
|
|
{
|
||
|
|
int key = 0;
|
||
|
|
// 方向
|
||
|
|
if(userInfo.diversion) AIN1 = 1, AIN1 = 0;
|
||
|
|
else AIN1 = 0, AIN1 = 1;
|
||
|
|
//userInfo.currentSpeed = Read_Encoder(2);
|
||
|
|
key = abs(Incremental_PI_B_Mode2(abs(userInfo.currentSpeed), 20));
|
||
|
|
PWMA = key;
|
||
|
|
OLED_ShowNum(0, 4, key,4, 16);
|
||
|
|
//PWMA = userInfo.pwm_val;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @description: 系统初始化
|
||
|
|
* @param {*}
|
||
|
|
* @return {*}
|
||
|
|
*/
|
||
|
|
void Sys_Init(void)
|
||
|
|
{
|
||
|
|
delay_init();
|
||
|
|
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
|
||
|
|
uart_init(115200);
|
||
|
|
PWM_Init(7199,0);
|
||
|
|
TIM3_Int_Init(5000, 71); // 定时器3
|
||
|
|
Encoder_Init_TIM2();
|
||
|
|
KEY_Init();
|
||
|
|
OLED_Init();
|
||
|
|
OLED_Clear();
|
||
|
|
userInfo.pwm_val = 6000;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @description: Oled显示
|
||
|
|
* @param {*}
|
||
|
|
* @return {*}
|
||
|
|
*/
|
||
|
|
void OLED_Display(void)
|
||
|
|
{
|
||
|
|
char str[30];
|
||
|
|
sprintf(str, "speed:%d,pwd_val:%d", abs(userInfo.currentSpeed), userInfo.pwm_val);
|
||
|
|
OLED_ShowString(0,0, str, 16);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|