128 lines
3.4 KiB
C
128 lines
3.4 KiB
C
#include "by_motion.h"
|
|
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include "by_debug.h"
|
|
#include "by_can.h"
|
|
|
|
#define D_X (0.25f) // 底盘 Y 轴上两轮中心的间距
|
|
#define D_Y (0.28f) // 底盘 X 轴上两轮中心的间距
|
|
#define RX_RY ((D_X + D_Y) / 2.f)
|
|
|
|
/**********************************************
|
|
* v_1 = v_{ty} + v_{tx} - (r_x + r_y) * \omega
|
|
* v_2 = v_{ty} - v_{tx} + (r_x + r_y) * \omega
|
|
* v_3 = v_{ty} + v_{tx} + (r_x + r_y) * \omega
|
|
* 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};
|
|
|
|
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_update_speed(void)
|
|
{
|
|
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;
|
|
|
|
// 根据安装方式调整轮子方向
|
|
v_wheel[1] *= -1.0f;
|
|
v_wheel[2] *= -1.0f;
|
|
|
|
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 电机控制 - 主循环中调用
|
|
*
|
|
*/
|
|
void by_motion_loop(void)
|
|
{
|
|
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;
|
|
} |