yuanjing/wheel
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
6b250ffc3d
wheel/main/mcpwm_bdc_control.c

433 lines
17 KiB
C
Raw Normal View History

first commit
2024-01-20 17:56:00 +08:00
/*
* SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <stdio.h>
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "driver/pulse_cnt.h"
#include "bdc_motor.h"
#include "pid_ctrl.h"
#include "ModbusS.h"
static const char *TAG = "bdc_control";
// Enable this config, we will print debug formated string, which in return can be captured and parsed by Serial-Studio
#define SERIAL_STUDIO_DEBUG CONFIG_SERIAL_STUDIO_DEBUG
#define BDC_MCPWM_TIMER_RESOLUTION_HZ 10000000 // 10MHz, 1 tick = 0.1us
#define BDC_MCPWM_FREQ_HZ 25000 // 25KHz PWM
#define BDC_MCPWM_DUTY_TICK_MAX (BDC_MCPWM_TIMER_RESOLUTION_HZ / BDC_MCPWM_FREQ_HZ) // maximum value we can set for the duty cycle, in ticks
#define BDC_MCPWM_GPIO_A 12
#define BDC_MCPWM_GPIO_B 13
#define BDC_MCPWM_GPIO_A1 14
#define BDC_MCPWM_GPIO_B1 15
#define BDC_MCPWM_GPIO_A2 16
#define BDC_MCPWM_GPIO_B2 17
//编码器对应IO口
#define BDC_ENCODER_GPIO_A 34
#define BDC_ENCODER_GPIO_B 35
#define BDC_ENCODER_GPIO_A1 33
#define BDC_ENCODER_GPIO_B1 26
#define BDC_CH1_LOW0_GPIO 1
#define BDC_CH1_HI0_GPIO 2
#define BDC_CH1_LOW1_GPIO 3
#define BDC_CH1_HI1_GPIO 4
#define BDC_CH1_AOUT_GPIO 5
#define BDC_CH1_AOUT_GPIO 6
#define BDC_CH0_LOW0_GPIO 7
#define BDC_CH0_HI0_GPIO 8
#define BDC_CH0_LOW1_GPIO 9
#define BDC_CH0_HI1_GPIO 10
//编码器脉冲计数误差范围
#define BDC_ENCODER_PCNT_HIGH_LIMIT 1000
#define BDC_ENCODER_PCNT_LOW_LIMIT -1000
#define BDC_PID_LOOP_PERIOD_MS 1000 // calculate the motor speed every 10ms
#define BDC_PID_EXPECT_SPEED 400 // expected motor speed, in the pulses counted by the rotary encoder
typedef struct
{
bdc_motor_handle_t motor;
pcnt_unit_handle_t pcnt_encoder;
pid_ctrl_block_handle_t pid_ctrl;
esp_timer_handle_t pid_loop_timer;
int accumu_count;
int report_pulses;
int id; /* 标识一个电机 */
} motor_control_context_t;
typedef struct
{
/* 从Mudbus服务器端得到设置参数 */
uint16_t speed; /* 速度大小 */
uint16_t foreward; /* 速度方向（注：现在只考虑前进和后退，因此0正1负 但是期望未来该值为一个-1->1之间的数，可以指导转弯） */
uint16_t inversion;
/* 发送到Modbus服务器显示的数据 */
uint16_t real_speedl; /* 真实速度 */
uint16_t real_forewardl; /* 真实方向 */
uint16_t real_motor_outputl; /* 真实电机输出 */
uint16_t real_motor_output_dirl; /* 注：这里将方向单独拆出只是为了在mudbus调试器上方便观看 */
uint16_t real_speedr; /* 真实速度 */
uint16_t real_forewardr; /* 真实方向 */
uint16_t real_motor_outputr; /* 真实电机输出 */
uint16_t real_motor_output_dirr; /* 注：这里将方向单独拆出只是为了在mudbus调试器上方便观看 */
} motor_control_t;
/* gWordVar寄存器组对应到motor_control_t的各个结构 */
motor_control_t *motor_control = (motor_control_t *)&gWordVar[0];
static bool example_pcnt_on_reach(pcnt_unit_handle_t unit, const pcnt_watch_event_data_t *edata, void *user_ctx)
{
int *accumu_count = (int *)user_ctx;
*accumu_count += edata->watch_point_value;
return false;
}
/* 定时器定时触发，使用PID算法控速
* 注意：速度以每次采样期间编码器计数个数为衡量标准
* 待修正：对于转弯的逻辑，可能左右轮的速度是期望速度的不同函数关系
*/
static void pid_loop_cb(void *args)
{
static int last_pulse_count = 0;
motor_control_context_t *ctx = (motor_control_context_t *)args;
pcnt_unit_handle_t pcnt_unit = ctx->pcnt_encoder;
pid_ctrl_block_handle_t pid_ctrl = ctx->pid_ctrl;
bdc_motor_handle_t motor = ctx->motor;
/* 从编码器获得计数，得到实际速度 */
int cur_pulse_count = 0;
pcnt_unit_get_count(pcnt_unit, &cur_pulse_count);
cur_pulse_count += ctx->accumu_count;
int real_pulses = cur_pulse_count - last_pulse_count; //新的脉冲数减去上一次的脉冲数为真实脉冲数
last_pulse_count = cur_pulse_count;
ctx->report_pulses = real_pulses;
/* 从Modbus服务器端获得期望速度 */
int speed = motor_control->foreward == 0 ? motor_control->speed : -motor_control->speed;
/* 计算速度误差->增量式PID->控制电机输出 */
float error = speed - real_pulses; /* 注意：这里使用的隐式类型转换 */
float new_speed = 0;
pid_compute(pid_ctrl, error, &new_speed);
bdc_motor_set_speed(motor, (uint32_t)new_speed);
/* 更新Modbus端的数据 */
if (ctx->id == 0)
{
motor_control->real_speedl = real_pulses > 0 ? real_pulses : -real_pulses;
motor_control->real_forewardl = real_pulses > 0 ? 0 : 1;
motor_control->real_motor_outputl = new_speed > 0 ? new_speed : -new_speed;
motor_control->real_motor_output_dirl = new_speed > 0 ? 0 : 1;
}
else if (ctx->id == 1)
{
motor_control->real_speedr = real_pulses > 0 ? real_pulses : -real_pulses;
motor_control->real_forewardr = real_pulses > 0 ? 0 : 1;
motor_control->real_motor_outputr = new_speed > 0 ? new_speed : -new_speed;
motor_control->real_motor_output_dirr = new_speed > 0 ? 0 : 1;
}
}
void bdc_motor_init(uint32_t bdc_mcpwm_gpio_a, uint32_t bdc_mcpwm_gpio_b, int group_id, bdc_motor_handle_t *motor_ret)
{
ESP_LOGI(TAG, "Create DC motor");
bdc_motor_config_t motor_config = {
.pwm_freq_hz = BDC_MCPWM_FREQ_HZ,
.pwma_gpio_num = bdc_mcpwm_gpio_a,
.pwmb_gpio_num = bdc_mcpwm_gpio_b,
};
bdc_motor_mcpwm_config_t mcpwm_config = {
.group_id = group_id,
.resolution_hz = BDC_MCPWM_TIMER_RESOLUTION_HZ, // 0.1us精密度
};
bdc_motor_handle_t motor = NULL;
ESP_ERROR_CHECK(bdc_motor_new_mcpwm_device(&motor_config, &mcpwm_config, &motor)); //根据当前误差检查
*motor_ret = motor;
}
void bdc_encoder_init(uint32_t bdc_encoder_gpioa, uint32_t bdc_encoder_gpiob, int *on_reach_callbackarg, pcnt_unit_handle_t *pcnt_unit_ret)
{
ESP_LOGI(TAG, "Init pcnt driver to decode rotary signal");
/* 实例化pcnt */
pcnt_unit_config_t unit_config = {
.high_limit = BDC_ENCODER_PCNT_HIGH_LIMIT,
.low_limit = BDC_ENCODER_PCNT_LOW_LIMIT,
};
pcnt_unit_handle_t pcnt_unit = NULL;
ESP_ERROR_CHECK(pcnt_new_unit(&unit_config, &pcnt_unit));
/* 设置过滤器宽度ns */
pcnt_glitch_filter_config_t filter_config = {
.max_glitch_ns = 1000,
};
ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit, &filter_config));
/* 设置通道a */
pcnt_chan_config_t chan_a_config = {
.edge_gpio_num = bdc_encoder_gpioa,
.level_gpio_num = bdc_encoder_gpiob,
};
pcnt_channel_handle_t pcnt_chan_a = NULL;
ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_a_config, &pcnt_chan_a));
/* 设置通道b */
pcnt_chan_config_t chan_b_config = {
.edge_gpio_num = bdc_encoder_gpiob,
.level_gpio_num = bdc_encoder_gpioa,
};
pcnt_channel_handle_t pcnt_chan_b = NULL;
ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_b_config, &pcnt_chan_b));
/* 配置两通道模式，实现两个通道的边沿都能计数 */
ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE));
ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE));
ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
/* 配置该pcnt的上下观察点，以及移除回调函数 */
ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_HIGH_LIMIT));
ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_LOW_LIMIT));
pcnt_event_callbacks_t pcnt_cbs = {
.on_reach = example_pcnt_on_reach, // accumulate the overflow in the callback
};
ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit, &pcnt_cbs, on_reach_callbackarg));
ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit));
ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit));
ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit));
*pcnt_unit_ret = pcnt_unit;
}
/* 注：只将不同的电机的不同参数作为接口留出，里面也有需要配置的共同参数 */
void bdc_pid_init(float p, float i, float d, pid_ctrl_block_handle_t *pid_ctrl_ret)
{
ESP_LOGI(TAG, "Create PID control block");
pid_ctrl_parameter_t pid_runtime_param = {
.kp = p,
.ki = i,
.kd = d,
.cal_type = PID_CAL_TYPE_INCREMENTAL, // 增量式pid
.max_output = BDC_MCPWM_DUTY_TICK_MAX - 1, //最大输出（占空比-1）、最小输出0
.min_output = 0,
.max_integral = 1000, //最大积分限制1000、最小积分限制-1000
.min_integral = -1000,
};
pid_ctrl_block_handle_t pid_ctrl = NULL;
pid_ctrl_config_t pid_config = {
.init_param = pid_runtime_param,
};
ESP_ERROR_CHECK(pid_new_control_block(&pid_config, &pid_ctrl));
*pid_ctrl_ret = pid_ctrl;
}
void bdc_ctrl_timer_init(motor_control_context_t *motor_ctrl_ctx, esp_timer_handle_t *pid_loop_timer_ret)
{
ESP_LOGI(TAG, "Create a timer to do PID calculation periodically");
const esp_timer_create_args_t periodic_timer_args = {
.callback = pid_loop_cb,
.arg = motor_ctrl_ctx,
.name = "pid_loop"
};
esp_timer_handle_t pid_loop_timer = NULL;
ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &pid_loop_timer));
*pid_loop_timer_ret = pid_loop_timer;
}
void bdc_motor_init_all(void)
{
/* 左轮 */
static motor_control_context_t motor_ctrl_ctxl = {
.id = 0, /* 左轮 */
.accumu_count = 0,
.pcnt_encoder = NULL,
.motor = NULL
};
bdc_motor_init(BDC_MCPWM_GPIO_A, BDC_MCPWM_GPIO_B, 0, &(motor_ctrl_ctxl.motor));
bdc_encoder_init(BDC_ENCODER_GPIO_A, BDC_ENCODER_GPIO_B, &(motor_ctrl_ctxl.accumu_count), &(motor_ctrl_ctxl.pcnt_encoder));
bdc_pid_init(0.6, 0.4, 0.2, &(motor_ctrl_ctxl.pid_ctrl));
bdc_ctrl_timer_init(&motor_ctrl_ctxl, &(motor_ctrl_ctxl.pid_loop_timer));
/* 右轮 */
static motor_control_context_t motor_ctrl_ctxr = {
.id = 0, /* 左轮 */
.accumu_count = 0,
.pcnt_encoder = NULL,
.motor = NULL
};
bdc_motor_init(BDC_MCPWM_GPIO_A, BDC_MCPWM_GPIO_B, 0, &(motor_ctrl_ctxr.motor));
bdc_encoder_init(BDC_ENCODER_GPIO_A1, BDC_ENCODER_GPIO_B1, &(motor_ctrl_ctxr.accumu_count), &(motor_ctrl_ctxr.pcnt_encoder));
bdc_pid_init(0.6, 0.4, 0.2, &(motor_ctrl_ctxr.pid_ctrl));
bdc_ctrl_timer_init(&motor_ctrl_ctxr, &(motor_ctrl_ctxr.pid_loop_timer));
ESP_LOGI(TAG, "Enable motor and forward");
ESP_ERROR_CHECK(bdc_motor_enable(motor_ctrl_ctxl.motor)); //启动电机
ESP_ERROR_CHECK(bdc_motor_forward(motor_ctrl_ctxl.motor)); //电机正转
ESP_ERROR_CHECK(bdc_motor_enable(motor_ctrl_ctxr.motor));
ESP_ERROR_CHECK(bdc_motor_forward(motor_ctrl_ctxr.motor));
ESP_LOGI(TAG, "Start motor speed loop"); //启动电机速度回路
ESP_ERROR_CHECK(esp_timer_start_periodic(motor_ctrl_ctxl.pid_loop_timer, BDC_PID_LOOP_PERIOD_MS * 1000)); //启动电机速度回路
ESP_ERROR_CHECK(esp_timer_start_periodic(motor_ctrl_ctxr.pid_loop_timer, BDC_PID_LOOP_PERIOD_MS * 1000));
}
void bdc_control_main(void)
{
static motor_control_context_t motor_ctrl_ctx = {
.accumu_count = 0,
.pcnt_encoder = NULL,
};
//配置用到的PWM
//创建一个直流电机，配置他的频率和IO口
ESP_LOGI(TAG, "Create DC motor");
bdc_motor_config_t motor_config = {
.pwm_freq_hz = BDC_MCPWM_FREQ_HZ,
.pwma_gpio_num = BDC_MCPWM_GPIO_A,
.pwmb_gpio_num = BDC_MCPWM_GPIO_B,
};
//配置MCpwm的标识符和lcd_clk的分辨率
bdc_motor_mcpwm_config_t mcpwm_config = {
.group_id = 0,
.resolution_hz = BDC_MCPWM_TIMER_RESOLUTION_HZ, // 0.1us精密度
};
bdc_motor_handle_t motor = NULL;
ESP_ERROR_CHECK(bdc_motor_new_mcpwm_device(&motor_config, &mcpwm_config, &motor)); //根据当前误差检查
motor_ctrl_ctx.motor = motor;
ESP_LOGI(TAG, "Init pcnt driver to decode rotary signal");
//配置（初始化）脉冲计数器，最高误差为1000，最低误差为-1000
pcnt_unit_config_t unit_config = {
.high_limit = BDC_ENCODER_PCNT_HIGH_LIMIT,
.low_limit = BDC_ENCODER_PCNT_LOW_LIMIT,
};
pcnt_unit_handle_t pcnt_unit = NULL;
ESP_ERROR_CHECK(pcnt_new_unit(&unit_config, &pcnt_unit));
pcnt_glitch_filter_config_t filter_config = {
.max_glitch_ns = 1000,
};
ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit, &filter_config));
//配置接编码器的IO口
pcnt_chan_config_t chan_a_config = {
.edge_gpio_num = BDC_ENCODER_GPIO_A,
.level_gpio_num = BDC_ENCODER_GPIO_B,
};
pcnt_channel_handle_t pcnt_chan_a = NULL;
ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_a_config, &pcnt_chan_a));
pcnt_chan_config_t chan_b_config = {
.edge_gpio_num = BDC_ENCODER_GPIO_B,
.level_gpio_num = BDC_ENCODER_GPIO_A,
};
pcnt_channel_handle_t pcnt_chan_b = NULL;
ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_b_config, &pcnt_chan_b));
//检查脉冲计数器通道的计数模式和计数方式
//电机相对安装，其中一个要取反
ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE));
ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE));
ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_HIGH_LIMIT));
ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_LOW_LIMIT));
pcnt_event_callbacks_t pcnt_cbs = {
.on_reach = example_pcnt_on_reach, // accumulate the overflow in the callback
};
ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit, &pcnt_cbs, &motor_ctrl_ctx.accumu_count));
ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit));
ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit));
ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit));
motor_ctrl_ctx.pcnt_encoder = pcnt_unit;
ESP_LOGI(TAG, "Create PID control block");
//配置PID运行参数
pid_ctrl_parameter_t pid_runtime_param = {
.kp = 0.6, // 0.6比例 系统响应速度
.ki = 0.4, // 0.4积分 消除系统的稳态误差
.kd = 0.2, // 0.2微分 抑制偏差向任何方向变化
.cal_type = PID_CAL_TYPE_INCREMENTAL, // pid增量控制
.max_output = BDC_MCPWM_DUTY_TICK_MAX - 1, //最大输出（占空比-1）、最小输出0
.min_output = 0,
.max_integral = 1000, //最大积分限制1000、最小积分限制-1000
.min_integral = -1000,
};
pid_ctrl_block_handle_t pid_ctrl = NULL;
pid_ctrl_config_t pid_config = {
.init_param = pid_runtime_param,
};
ESP_ERROR_CHECK(pid_new_control_block(&pid_config, &pid_ctrl));
motor_ctrl_ctx.pid_ctrl = pid_ctrl;
ESP_LOGI(TAG, "Create a timer to do PID calculation periodically");
const esp_timer_create_args_t periodic_timer_args = {
.callback = pid_loop_cb,
.arg = &motor_ctrl_ctx, //回调PID的参数
.name = "pid_loop" //定时器名pid_loop
};
esp_timer_handle_t pid_loop_timer = NULL;
ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &pid_loop_timer));
ESP_LOGI(TAG, "Enable motor");
ESP_ERROR_CHECK(bdc_motor_enable(motor)); //启动电机
ESP_LOGI(TAG, "Forward motor");
ESP_ERROR_CHECK(bdc_motor_forward(motor)); //电机正转
ESP_LOGI(TAG, "Start motor speed loop"); //启动电机速度回路
ESP_ERROR_CHECK(esp_timer_start_periodic(pid_loop_timer, BDC_PID_LOOP_PERIOD_MS * 1000)); //启动电机速度回路
while (1)
{
vTaskDelay(pdMS_TO_TICKS(100));
// the following logging format is according to the requirement of serial-studio frame format
// also see the dashboard config file `serial-studio-dashboard.json` for more information
#if SERIAL_STUDIO_DEBUG
printf("/*%d*/\r\n", motor_ctrl_ctx.report_pulses);
#endif
}
}
Reference in New Issue Copy Permalink