xushenghao
/
motor_f103
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增量PID移植完成

This commit is contained in:
许晟昊 2024-12-08 16:31:15 +08:00
parent 8880969627
commit ef8d639bda
15 changed files with 1518 additions and 1340 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Src/gpio.c
View File

@ -134,7 +134,7 @@ void MX_GPIO_Init(void)
LL_GPIO_SetPinMode(MOTOR_A_GPIO_Port, MOTOR_A_Pin, LL_GPIO_MODE_FLOATING);
/* EXTI interrupt init*/
NVIC_SetPriority(EXTI0_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
NVIC_SetPriority(EXTI0_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),4, 0));
NVIC_EnableIRQ(EXTI0_IRQn);
}

2
Core/Src/stm32f1xx_it.c
View File

@ -281,8 +281,8 @@ void TIM3_IRQHandler(void)
/* USER CODE BEGIN TIM3_IRQn 1 */
if (IS_TIM_IT_FLAG(WORK_TIM))
{
TIM_IRQ_HANDLER(WORK_TIM);
work_process();
TIM_IRQ_HANDLER(WORK_TIM);
}
/* USER CODE END TIM3_IRQn 1 */
}

2
Core/Src/tim.c
View File

@ -122,7 +122,7 @@ void MX_TIM3_Init(void)
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
/* TIM3 interrupt Init */
NVIC_SetPriority(TIM3_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),1, 0));
NVIC_SetPriority(TIM3_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),3, 0));
NVIC_EnableIRQ(TIM3_IRQn);
/* USER CODE BEGIN TIM3_Init 1 */

8
MDK-ARM/motor.uvoptx
View File

@ -213,6 +213,12 @@
<pszMrulep></pszMrulep>
<pSingCmdsp></pSingCmdsp>
<pMultCmdsp></pMultCmdsp>
<SystemViewers>
<Entry>
<Name>System Viewer\GPIOB</Name>
<WinId>35905</WinId>
</Entry>
</SystemViewers>
</TargetOption>
</Target>
@ -810,7 +816,7 @@
<Group>
<GroupName>User/lib/control</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

2591
MDK-ARM/motor/motor.hex
View File

File diff suppressed because it is too large Load Diff

7
User/application/app.c
View File

@ -32,7 +32,12 @@ static uint8_t business_inspection(struct flow *fl)
ssd1306_update_screen();
break;
case WORK_MOTOR_SPEED:
/* code */
ssd1306_clear_buffer();
ssd1306_f8x16_string(0, 0, " TAR:");
ssd1306_f8x16_number(48, 0, work.target_speed, 0);
ssd1306_f8x16_string(0, 2, " CUR:");
ssd1306_f8x16_number(48, 2, work.encoder_show, 0);
ssd1306_update_screen();
break;
default:
break;

27
User/application/key.c
View File

@ -1,6 +1,8 @@
#include "key.h"
#include "btn.h"
#include "work.h"
#define INVALID_BUTTON_TICKS 200 // 无效按键时间 毫秒
static uint32_t key_start_ticks = 0;
/* 按钮 */
struct Button key_1;
struct Button key_2;
@ -9,8 +11,24 @@ struct Button key_4;
struct Button key_5;
struct Button key_6;
static BOOL allow_condition(void)
{
/**
* key的初始化在LCD板子上电之前，因为是低电平有效，所以会误判为按键按下
*/
if (sys_millis() - key_start_ticks < INVALID_BUTTON_TICKS) // 仿真的时候按键会有毛刺，在xx秒之后按下有效
{
return FALSE;
}
return TRUE;
}
static uint8_t read_button_gpio(button_id_e button_id)
{
if (allow_condition() == FALSE)
{
return ACTIVE_LEVEL_LOW;
}
switch (button_id)
{
case KEY_ADD:
@ -20,7 +38,7 @@ static uint8_t read_button_gpio(button_id_e button_id)
case KEY_S:
return GPIO_READ(KEY_S_GPIO_Port, KEY_S_Pin);
default:
return ACTIVE_LEVEL_HIGH;
return ACTIVE_LEVEL_LOW;
}
}
@ -41,13 +59,14 @@ static void key_3_press_down_handler(void *btn)
void key_init(void)
{
key_start_ticks = sys_millis();
GPIO_SET_INPUT(KEY_ADD_GPIO_Port, KEY_ADD_Pin);
GPIO_SET_INPUT(KEY_SUB_GPIO_Port, KEY_SUB_Pin);
GPIO_SET_INPUT(KEY_S_GPIO_Port, KEY_S_Pin);
button_init(&key_1, read_button_gpio, ACTIVE_LEVEL_LOW, KEY_ADD, KEY_ADD);
button_init(&key_2, read_button_gpio, ACTIVE_LEVEL_LOW, KEY_SUB, KEY_SUB);
button_init(&key_3, read_button_gpio, ACTIVE_LEVEL_LOW, KEY_S, KEY_S);
button_init(&key_1, read_button_gpio, ACTIVE_LEVEL_HIGH, KEY_ADD, KEY_ADD);
button_init(&key_2, read_button_gpio, ACTIVE_LEVEL_HIGH, KEY_SUB, KEY_SUB);
button_init(&key_3, read_button_gpio, ACTIVE_LEVEL_HIGH, KEY_S, KEY_S);
button_attach(&key_1, PRESS_DOWN, key_1_press_down_handler);
button_attach(&key_2, PRESS_DOWN, key_2_press_down_handler);

99
User/application/work.c
View File

@ -2,6 +2,9 @@
#include "board.h"
#define OUTPUT_INFORMATION_CYCLE_BASE 100
#define DISTANCE 1000
#define SPEED 80
#define TARGET_POS 500
#define TARGET_SPEED 20
work_t work;
/**
@ -22,6 +25,7 @@ static void output_information(void)
}
else if (work.type == WORK_MOTOR_SPEED)
{
rst = snprintf(data, ARRAY_LEN(data), "%d,%d,%f\r\n", work.target_speed, work.encoder_show, work.pwm_percent);
}
else
{
@ -44,14 +48,27 @@ static void pwm_map(void)
static void motor_pos(void)
{
work.pid.sub_type = PID_SUB_TYPE_POSITION;
work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_pos, work.encoder_cnt);
int32_t code = work.encoder_cnt;
work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_pos, code);
set_motor_pwm(work.pwm_percent);
}
static void motor_speed(void)
{
work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
int32_t code = work.encoder_cnt;
if (ABS(work.target_speed - work.encoder_show) > 20)
{
work.target_speed_filter = lpf_update(&work.target_speed_lpf, work.target_speed);
}
else
{
work.target_speed_filter = work.target_speed;
}
work.encoder_show = lpf_update(&work.encoder_lpf, work.encoder_cnt);
work.encoder_cnt = 0;
work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_speed_filter + 1, code); // 补偿1
set_motor_pwm(work.pwm_percent);
}
static void pwm_map_key_handle(button_id_e id)
@ -99,7 +116,7 @@ static void motot_pos_key_handle(button_id_e id)
}
break;
case KEY_S:
work.target_pos = 500;
work.target_pos = TARGET_POS;
break;
default:
break;
@ -108,6 +125,28 @@ static void motot_pos_key_handle(button_id_e id)
static void motor_speed_key_handle(button_id_e id)
{
int16_t min = TARGET_SPEED, max = SPEED;
int16_t step = 5;
switch (id)
{
case KEY_ADD:
if (work.target_speed < max)
{
work.target_speed += step;
}
break;
case KEY_SUB:
if (work.target_speed > min)
{
work.target_speed -= step;
}
break;
case KEY_S:
work.target_speed = TARGET_SPEED;
break;
default:
break;
}
}
void work_key_handle_cb(button_id_e id)
@ -162,7 +201,7 @@ void work_process(void)
void work_encoder_exti(void)
{
if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin))
if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin) == 1)
{
work.encoder_cnt++;
}
@ -179,24 +218,60 @@ void work_init(void)
PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, 0);
work.pwm_feq = PWM_GET_FREQ(PWM_TIM);
work.pwm_arr = PWM_GET_ARR(PWM_TIM);
work.type = WORK_MOTOR_POS;
work.pid_params.kp = 1;
work.pid_params.ki = 0.01f;
work.pid_params.kd = 10;
work.pid_params.dead_zone = 0.5;
work.type = WORK_MOTOR_SPEED;
// PID初始化
{
work.pid.type = PID_TYPE_FUZZY;
work.pid.sub_type = PID_SUB_TYPE_POSITION;
work.pid.pid_u.fuzzy.deadzone_dir = DEAD_ZONE_BOTH;
work.pid_params.dead_zone = 1;
if (work.type == WORK_MOTOR_POS)
{
work.pid_params.kp = 1;
work.pid_params.ki = 0.01f;
work.pid_params.kd = 10;
work.pid.sub_type = PID_SUB_TYPE_POSITION;
}
else
{
work.pid_params.kp = 0.01;
work.pid_params.ki = 0.1f;
work.pid_params.kd = 0;
work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
}
pid_constructor(&work.pid);
if (work.type == WORK_MOTOR_POS)
{
work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
work.pid_params.kd, work.pid_params.dead_zone, 0, -90, 90); // 电机输出不要超过90
work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, TRUE);
work.position = &work.pid.pid_u.fuzzy.pri_u.position;
}
else
{
work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
work.pid_params.kd, work.pid_params.dead_zone, 0, -1 * ((int32_t)SPEED), SPEED);
work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, FALSE);
work.increment = &work.pid.pid_u.fuzzy.pri_u.increment;
work.encoder_lpf.alpha = work.pid_params.ki * 1.5f;
lpf_init(&work.encoder_lpf);
work.target_speed_lpf.alpha = work.pid_params.ki * 1.5f;
lpf_init(&work.target_speed_lpf);
// 改变定时器周期
uint16_t arr = LL_TIM_GetAutoReload(WORK_TIM);
LL_TIM_SetAutoReload(WORK_TIM, arr * 10 - 1);
}
}
work.target_pos = 500;
work.target_pos = TARGET_POS;
work.target_speed = TARGET_SPEED;
work.timer_cycle = TIM_CYCLE(WORK_TIM);
return;
}

10
User/application/work.h
View File

@ -4,7 +4,7 @@
#include "main.h"
#include "key.h"
#include "pid.h"
#include "filter.h"
typedef enum
{
// PWM占空比映射到电机
@ -21,10 +21,13 @@ typedef struct
uint8_t timer_cycle; // 定时器周期
uint32_t enter_cnt; // 进入次数
int32_t encoder_cnt; // 编码器位置
int32_t encoder_show; // 滤波后的编码器位置
float32 pwm_percent; // PWM占空比
uint32_t pwm_feq; // PWM频率
uint16_t pwm_arr; // PWM自动重装载值
pid_t pid;
pid_common_position_t *position;
pid_common_increment_t *increment;
struct
{
float32 kp;
@ -34,7 +37,10 @@ typedef struct
} pid_params;
int32_t target_pos; // 目标位置
float32 target_speed; // 目标速度
int32_t target_speed; // 目标速度
int32_t target_speed_filter;
lpf_t encoder_lpf;
lpf_t target_speed_lpf;
} work_t;
extern work_t work;

15
User/board/board.c
View File

@ -47,6 +47,21 @@ void set_motor_pwm(float32 percent)
PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, ABS(percent));
}
void set_motor_pwm_arr(int16_t arr)
{
if (arr > 0)
{
GPIO_SET(AIN1_GPIO_Port, AIN1_Pin);
GPIO_RESET(AIN2_GPIO_Port, AIN2_Pin);
}
else
{
GPIO_RESET(AIN1_GPIO_Port, AIN1_Pin);
GPIO_SET(AIN2_GPIO_Port, AIN2_Pin);
}
PWM_SET_COMPARE(PWM_TIM, PWM_CHANNEL, ABS(arr));
}
/**
* @brief 通过UART发送数据
*

2
User/board/board.h
View File

@ -15,5 +15,5 @@ extern uart_t *uart;
void board_init(void);
void uart_send(uint8_t *data, uint16_t len);
void set_motor_pwm(float32 percent);
void set_motor_pwm_arr(int16_t arr);
#endif

4
User/lib/control/inc/pid.h
View File

@ -7,8 +7,8 @@
typedef enum
{
PID_SUB_TYPE_POSITION, // 位置式
PID_SUB_TYPE_INCREMENT, // 增量式
PID_SUB_TYPE_POSITION = 1, // 位置式
PID_SUB_TYPE_INCREMENT = 2, // 增量式
} pid_sub_type_e;
typedef enum
{

1
User/lib/control/src/pid.c
View File

@ -13,6 +13,7 @@ void pid_constructor(pid_t *self)
pid_neural_constructor(&self->pid_u.neural);
break;
case PID_TYPE_FUZZY:
DBG_ASSERT(self->sub_type != 0 __DBG_LINE);
self->pid_u.fuzzy.sub_type = self->sub_type;
pid_fuzzy_constructor(&self->pid_u.fuzzy);
break;

6
User/lib/control/src/pid_fuzzy.c
View File

@ -13,17 +13,17 @@
#define PL 3
// 定义偏差E的范围，因为设置了非线性区间，误差在10时才开始进行PID调节，这里E的范围为10
#define MAXE (10)
#define MAXE (90)
#define MINE (-MAXE)
// 定义EC的范围，因为变化非常缓慢！，每次的EC都非常小，这里可以根据实际需求来调整，
#define MAXEC (10)
#define MAXEC (90)
#define MINEC (-MAXEC)
// 定义e,ec的量化因子
#define KE 3 / MAXE
#define KEC 3 / MAXEC
// 定义输出量比例因子
#define KUP 3.0f // 这里只使用了模糊PID的比例增益
#define KUP 1.0f // 这里只使用了模糊PID的比例增益
#define KUI 0.0f
#define KUD 0.0f

4
motor.ioc
View File

@ -71,7 +71,7 @@ NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.DMA1_Channel4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
NVIC.DMA1_Channel5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.EXTI0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
NVIC.EXTI0_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true
NVIC.ForceEnableDMAVector=true
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
@ -81,7 +81,7 @@ NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false
NVIC.TIM1_UP_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true
NVIC.TIM3_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true
NVIC.TIM3_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true
NVIC.USART1_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
PA0-WKUP.GPIOParameters=GPIO_Label