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

feat: 增加上下位机通信接口 (底盘速度和位置控制)

Browse Source
This commit is contained in:
bmy
2024-04-24 23:22:21 +08:00
parent 7123fb2f25
commit 1c3131e9c7
14 changed files with 560 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
app/by_motion.c
View File

@@ -2,13 +2,12 @@
#include <math.h>
#include <string.h>
#include "by_debug.h"
#include "by_can.h"
#define D_X (0.18f) // 底盘 Y 轴上两轮中心的间距
#define D_Y (0.25f) // 底盘 X 轴上两轮中心的间距
#define RX_RY ((D_X + D_Y) / 2.f)
#define FP2U16_SCALE (1000.f) // 浮点转存为整数缩放尺度，所有设备该参数需对应
#define FP2U16(x) ((int16_t)(x * FP2U16_SCALE)) // 浮点转存为整数
#define D_X (0.18f) // 底盘 Y 轴上两轮中心的间距
#define D_Y (0.25f) // 底盘 X 轴上两轮中心的间距
#define RX_RY ((D_X + D_Y) / 2.f)
/**********************************************
* v_1 = v_{ty} + v_{tx} - (r_x + r_y) * \omega
@@ -17,11 +16,20 @@
* v_4 = v_{ty} - v_{tx} - (r_x + r_y) * \omega
**********************************************/
/*** 控制模式 ***/
uint8_t control_mode = 0; // 0-速度模式 1-位置模式
/*** 位置控制 ***/
uint8_t control_timer = 0; // 位置控制定时器状态
uint32_t control_timer_cnt = 0; // 位置控制计数
/*** 各轮转速，左上角为 1 号，顺时针标号 ***/
float v_wheel[4] = {0.f};
/** 目标速度 **/
motion_speed_type motion_speed_struct;
motion_speed_type motion_speed_struct_last;
motion_time_type motion_time_struct;
/** 下发数据包 **/
int16_t motion_speed_data[4] = {0};
@@ -29,26 +37,88 @@ int16_t motion_speed_data[4] = {0};
void by_motion_init(void)
{
memset(&motion_speed_struct, 0, sizeof(motion_speed_struct));
memset(&motion_speed_struct_last, 0, sizeof(motion_speed_struct_last));
memset(&motion_time_struct, 0, sizeof(motion_time_struct));
memset(motion_speed_data, 0, sizeof(motion_speed_data));
memset(v_wheel, 0, sizeof(v_wheel));
by_can_send_stdd(0x01, (uint8_t *)&motion_speed_data, 8, 100);
}
void by_motion_calc_speed(motion_speed_type *speed)
void by_motion_update_speed(void)
{
motion_speed_struct = *speed;
const motion_speed_type *speed = &motion_speed_struct;
v_wheel[0] = speed->v_x + speed->v_y - RX_RY * speed->v_w;
v_wheel[1] = speed->v_x - speed->v_y + RX_RY * speed->v_w;
v_wheel[2] = speed->v_x + speed->v_y + RX_RY * speed->v_w;
v_wheel[3] = speed->v_x - speed->v_y - RX_RY * speed->v_w;
for (uint8_t i = 0; i < 4; i++) {
motion_speed_data[i] = (int16_t)v_wheel[i];
LOGD("MOTION#SPD wheel[%d] - %d", i, motion_speed_data[i]);
}
by_can_send_stdd(0x01, (uint8_t *)&motion_speed_data, 8, 100);
LOGD("MOTION#SPD updated");
}
/**
* @brief
* @brief 电机控制 - 主循环中调用
*
*/
void by_motion_pack_speed(void)
void by_motion_loop(void)
{
for (uint8_t i = 0; i < 4; i++) {
motion_speed_data[i] = FP2U16(v_wheel[i]);
if (control_mode == 0) { // 速度控制模式
memset(&motion_time_struct, 0, sizeof(motion_time_struct));
} else { // 位置控制模式
if (0 == motion_time_struct.t_x) {
motion_speed_struct.v_x = 0;
}
if (0 == motion_time_struct.t_y) {
motion_speed_struct.v_y = 0;
}
if (0 == motion_time_struct.t_w) {
motion_speed_struct.v_w = 0;
}
}
if ((motion_speed_struct.v_x != motion_speed_struct_last.v_x) || (motion_speed_struct.v_y != motion_speed_struct_last.v_y) || (motion_speed_struct.v_w != motion_speed_struct_last.v_w)) {
by_motion_update_speed();
memcpy(&motion_speed_struct_last, &motion_speed_struct, sizeof(motion_speed_type));
}
}
/**
* @brief 定时回调
*
*/
void by_motion_timer_handle(void)
{
if (control_mode == 0) {
motion_time_struct.t_x = 0;
motion_time_struct.t_y = 0;
motion_time_struct.t_w = 0;
} else {
if (motion_time_struct.t_x > 0) {
motion_time_struct.t_x--;
}
if (motion_time_struct.t_y > 0) {
motion_time_struct.t_y--;
}
if (motion_time_struct.t_w > 0) {
motion_time_struct.t_w--;
}
}
}
/**
* @brief 设置电机控制模式
*
* @param mode 0-速度模式 1-位置模式
*/
void by_motion_set_mode(uint8_t mode)
{
control_mode = mode;
}