btl143/BC3D-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 增加can总线和步进电机指令绑定

Browse Source
This commit is contained in:
bmy
2024-05-01 12:16:55 +08:00
parent 7ef6f87c84
commit fd047b601c
15 changed files with 190 additions and 345 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
app/by_can.c
View File

@@ -26,14 +26,16 @@ by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len,
transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); /* 将以上待发送报文写入发送邮箱并请求发送 */
/* 等待该邮箱发送成功—对应邮箱发送成功标志置起 */
while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) != CAN_TX_STATUS_SUCCESSFUL) {
// LOGD("CAN#SEND: timeout=%d", timeout);
if (0 == timeout--) {
LOGW("CAN#TIMEOUT: ID=0x%x", id);
if (timeout) {
while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) != CAN_TX_STATUS_SUCCESSFUL) {
// LOGD("CAN#SEND: timeout=%d", timeout);
if (0 == timeout--) {
LOGW("CAN#TIMEOUT: ID=0x%x", id);
return BY_ERROR;
return BY_ERROR;
}
DWT_Delay(10);
}
DWT_Delay(10);
}
return BY_SUCCESS;

38
app/by_messy.c Normal file
View File

@@ -0,0 +1,38 @@
#include "by_messy.h"
#include <string.h>
#include "by_stepper.h"
#include "by_can.h"
void by_messy_loop(void)
{
}
void by_messy_int(void)
{
can_rx_message_type can_rx_message_struct;
can_message_receive(CAN1, CAN_RX_FIFO0, &can_rx_message_struct);
// 接收到位置控制帧
if (0x006 == can_rx_message_struct.standard_id) {
uint8_t mode = 0;
uint8_t speed = 0;
float position_temp = 0;
memcpy(&mode, can_rx_message_struct.data, 1);
memcpy(&speed, can_rx_message_struct.data + 1, 1);
memcpy(&position_temp, can_rx_message_struct.data + 2, 4);
by_stepper_set_speed((stepper_speed_t)speed);
if (0 == mode) {
by_stepper_set_position_millimeter(position_temp);
} else if (1 == mode) {
by_stepper_add_position_millimeter(position_temp);
}
} else if (0x007 == can_rx_message_struct.standard_id) {
if (CAN_TFT_REMOTE == can_rx_message_struct.frame_type) {
by_can_send_stdd(0x007, &running_flag, 1, 0);
}
}
}

9
app/by_messy.h Normal file
View File

@@ -0,0 +1,9 @@
#ifndef _BY_MESSY_H__
#define _BY_MESSY_H__
#include "at32f413.h"
extern void by_messy_loop(void);
extern void by_messy_int(void);
#endif

108
app/by_motion.c
View File

@@ -1,108 +0,0 @@
#include "by_motion.h"
#include "pid.h"
#include "dwt_delay.h"
#include "by_utils.h"
#include "by_debug.h"
#define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15)
#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)
by_motor_param param_m1;
by_motor_param param_m2;
PID_TypeDef pid_m1;
PID_TypeDef pid_m2;
int16_t speed_m1;
int16_t speed_m2;
uint8_t motion_enable_flag;
void by_motion_set_pwm_m1(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
pwm_duty += 499;
// 互补 pwm 输出，499 为中值
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
}
void by_motion_set_pwm_m2(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
pwm_duty += 499;
// 互补 pwm 输出，499 为中值
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
}
int16_t by_motion_get_speed_m1(void)
{
param_m1.real_speed = (float)(-1 * (int16_t)tmr_counter_value_get(TMR2));
tmr_counter_value_set(TMR2, 0);
return (int16_t)param_m1.real_speed;
}
int16_t by_motion_get_speed_m2(void)
{
param_m2.real_speed = (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
tmr_counter_value_set(TMR3, 0);
return (int16_t)param_m2.real_speed;
}
void by_motion_set_speed_m1(int16_t speed)
{
param_m1.target_speed = speed;
}
void by_motion_set_speed_m2(int16_t speed)
{
param_m2.target_speed = speed;
}
void by_motion_init(void)
{
motion_enable_flag = 0;
by_motion_set_pwm_m1(0);
by_motion_set_pwm_m2(0);
/* set default parameters */
param_m1.Kp = BY_MOTION_DEFAULT_KP_M1;
param_m1.Ki = BY_MOTION_DEFAULT_KI_M1;
param_m1.Kd = BY_MOTION_DEFAULT_KD_M1;
param_m2.Kp = BY_MOTION_DEFAULT_KP_M2;
param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;
/* load parameters form ee */
// TODO
PID(&pid_m1, &param_m1.real_speed, &param_m1.out_pwm, &param_m1.target_speed, param_m1.Kp, param_m1.Ki, param_m1.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&pid_m1, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pid_m1, 10);
PID_SetOutputLimits(&pid_m1, -400, 400);
PID(&pid_m2, &param_m2.real_speed, &param_m2.out_pwm, &param_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pid_m2, 10);
PID_SetOutputLimits(&pid_m2, -400, 400);
param_m1.target_speed = 17;
motion_enable_flag = 1;
DRV_ENABLE();
}
void by_motion_run(void)
{
if (motion_enable_flag) {
by_motion_get_speed_m1();
by_motion_get_speed_m2();
PID_Compute(&pid_m1);
PID_Compute(&pid_m2);
LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
// by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
}
}

34
app/by_motion.h
View File

@@ -1,34 +0,0 @@
#ifndef _BY_MOTION_H__
#define _BY_MOTION_H__
#include "at32f413.h"
typedef struct by_motor_param {
float Kp;
float Ki;
float Kd;
float real_speed;
float target_speed;
float out_pwm;
} by_motor_param;
#define BY_MOTION_DEFAULT_KP_M1 (13.0f)
#define BY_MOTION_DEFAULT_KI_M1 (5.0f)
#define BY_MOTION_DEFAULT_KD_M1 (0.1f)
#define BY_MOTION_DEFAULT_KP_M2 (1.0f)
#define BY_MOTION_DEFAULT_KI_M2 (0.0f)
#define BY_MOTION_DEFAULT_KD_M2 (0.0f)
extern void by_motion_init(void);
extern void by_motion_run(void);
extern void by_motion_set_pwm_m1(int32_t pwm_duty);
extern void by_motion_set_pwm_m2(int32_t pwm_duty);
extern int16_t by_motion_get_speed_m1(void);
extern int16_t by_motion_get_speed_m2(void);
extern void by_motion_set_speed_m1(int16_t speed);
extern void by_motion_set_speed_m2(int16_t speed);
extern int16_t speed_m1;
extern int16_t speed_m2;
#endif

46
app/by_param.c
View File

@@ -1,46 +0,0 @@
#include "by_param.h"
#include <string.h>
#include "eeprom.h"
#include "by_debug.h"
by_param_type param_index[BY_PARAM_NUM_MAX];
uint16_t param_num = 0;
by_error_status by_param_init(void)
{
param_num = 0;
return BY_SUCCESS;
}
by_error_status by_param_reg(uint16_t reg_addr, uint16_t *data_ptr, const char *data_str)
{
param_index[param_num].reg_addr = reg_addr;
param_index[param_num].data_ptr = data_ptr;
strncpy(param_index[param_num].data_str, data_str, sizeof(param_index[param_num].data_str));
LOGD("reg data [%s]: addr-0x%04x", param_index[param_num].data_str, param_index[param_num].reg_addr);
param_num++;
return BY_SUCCESS;
}
by_error_status by_param_load_from_ee(uint16_t reg_addr)
{
for (int i = 0; i < param_num; i++) {
if (param_index[i].reg_addr == reg_addr) {
flash_ee_data_read(reg_addr, param_index[i].data_ptr);
return BY_SUCCESS;
}
}
return BY_ERROR;
}
by_error_status by_param_save_to_ee(uint16_t reg_addr)
{
for (int i = 0; i < param_num; i++) {
if (param_index[i].reg_addr == reg_addr) {
flash_ee_data_write(reg_addr, *param_index[i].data_ptr);
return BY_SUCCESS;
}
}
return BY_ERROR;
}

19
app/by_param.h
View File

@@ -1,19 +0,0 @@
#ifndef _BY_PARAM_H__
#define _BY_PARAM_H__
#include "at32f413.h"
#include "by_utils.h"
typedef struct by_param_type {
uint16_t reg_addr;
uint16_t *data_ptr;
char data_str[20];
} by_param_type;
#define BY_PARAM_NUM_MAX (20)
by_error_status by_param_init(void);
by_error_status by_param_reg(uint16_t reg_addr, uint16_t *data_ptr, const char *data_str);
#endif

52
app/by_stepper.c
View File

@@ -22,6 +22,7 @@ int64_t position_aim = 0;
int64_t position_offset = 0;
uint8_t running_flag = 0;
uint8_t reset_flag = 0;
// TODO 是否增加一个动作列表？一般都比较简单吧
@@ -50,6 +51,8 @@ void by_stepper_set_speed(stepper_speed_t speed)
void by_stepper_run(void)
{
// 根据预设条件发送脉冲
gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE); // EN
gpio_bits_write(GPIOA, GPIO_PINS_8, by_stepper.dir ? TRUE : FALSE); // DIR
gpio_bits_write(GPIOB, GPIO_PINS_15, !gpio_output_data_bit_read(GPIOB, GPIO_PINS_15)); // CLK
DWT_Delay(40U * (uint16_t)by_stepper.speed);
@@ -66,6 +69,14 @@ void by_stepper_stop(uint8_t hold)
void by_stepper_set_position_millimeter(float distance)
{
if (!running_flag) {
// 设置距离较小时直接复位，减少累计定位误差
if (distance < 3 && distance > -3) {
reset_flag = 1;
running_flag = 1;
return;
}
gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE); // EN
position_aim = (int64_t)(distance * ENCODER_PULSE_PER_MILLIMETER);
position_offset = position_aim - position;
running_flag = 1;
@@ -75,6 +86,7 @@ void by_stepper_set_position_millimeter(float distance)
void by_stepper_add_position_millimeter(float distance)
{
if (!running_flag) {
gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE); // EN
position_aim = position + (int64_t)(distance * ENCODER_PULSE_PER_MILLIMETER);
position_offset = position_aim - position;
running_flag = 1;
@@ -112,7 +124,9 @@ void by_stepper_init(void)
DWT_Delay(40U * (uint16_t)by_stepper.speed);
}
position = 0; // 位置归零
position = 0; // 位置归零
position_aim = 0;
position_offset = 0;
tmr_counter_value_set(TMR3, 0);
LOGD("by_stepper init ok");
@@ -125,21 +139,31 @@ void by_stepper_loop(void)
tmr_counter_value_set(TMR3, 0);
if (running_flag) {
if (position_offset > 0) {
by_stepper_set_dir(0);
by_stepper_run();
} else if (position_offset < 0) {
by_stepper_set_dir(1);
by_stepper_run();
if (reset_flag) // 复位模式
{
by_stepper_init();
running_flag = 0;
reset_flag = 0;
} else // 正常模式
{
if (position_offset > 0) {
by_stepper_set_dir(0);
by_stepper_run();
} else if (position_offset < 0) {
by_stepper_set_dir(1);
by_stepper_run();
}
// 当定位误差小于阈值时，停止运动。否则重新计算定位误差
if (abs_s64(position_aim - position) < ENCODER_ERROR_THRESHOLD) {
running_flag = 0;
position_offset = 0;
} else {
position_offset = position_aim - position;
}
}
// 当定位误差小于阈值时，停止运动。否则重新计算定位误差
if (abs_s64(position_aim - position) < ENCODER_ERROR_THRESHOLD) {
running_flag = 0;
position_offset = 0;
} else {
position_offset = position_aim - position;
}
} else {
by_stepper_stop(0);
}

1
app/by_stepper.h
View File

@@ -25,5 +25,6 @@ extern void by_stpepper_int(void);
extern int64_t position;
extern int64_t position_aim;
extern int64_t position_offset;
extern uint8_t running_flag;
#endif