[release] 发布8DOF串联足中期检查版本
This commit is contained in:
@@ -0,0 +1,284 @@
|
||||
/**
|
||||
* @file lingzu_task.c
|
||||
* @brief 主控制任务 — 适配直接电机角 IK
|
||||
*
|
||||
* 改动:
|
||||
* - IK 直接输出无符号电机角 (motor_thigh, motor_shank)
|
||||
* - 偏移量校准: IK(0, PRONE_Z) 应输出 ≈ (0, 0)
|
||||
* offset = encoder_reading - IK_output*sign
|
||||
* - 废除 PostureConfig 相关调用
|
||||
* - Quad_Prone_Crawl_Ready 合并为 Quad_Prone
|
||||
*/
|
||||
|
||||
#include "lingzu_task.h"
|
||||
#include "lingzu_motor.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "rm_hal_lib.h"
|
||||
#include <math.h>
|
||||
#include "Movement.h"
|
||||
#include "gait_config.h"
|
||||
#include "uart_task.h"
|
||||
#include "uart_device.h"
|
||||
|
||||
/* #define CALIBRATE_ZERO_POINT */
|
||||
|
||||
#define MOTOR_COUNT 8
|
||||
#define PI 3.1415926535f
|
||||
|
||||
typedef struct {
|
||||
CAN_HandleTypeDef *hcan;
|
||||
Motor_CAN_Send_Struct *tx;
|
||||
Motor_CAN_Recieve_Struct *rx;
|
||||
float sign; /* -1(左) 或 +1(右) */
|
||||
uint8_t is_thigh; /* 1=大腿, 0=小腿 */
|
||||
} MotorHandle;
|
||||
|
||||
static MotorHandle motors[MOTOR_COUNT];
|
||||
|
||||
static void motors_init_handles(void)
|
||||
{
|
||||
motors[0] = (MotorHandle){ &hcan1, &CAN_1.ID_1_Motor_send, &CAN_1.ID_1_Motor_recieve, -1.0f, 1 }; /* FL thigh */
|
||||
motors[1] = (MotorHandle){ &hcan1, &CAN_1.ID_2_Motor_send, &CAN_1.ID_2_Motor_recieve, -1.0f, 0 }; /* FL shank */
|
||||
motors[2] = (MotorHandle){ &hcan1, &CAN_1.ID_3_Motor_send, &CAN_1.ID_3_Motor_recieve, 1.0f, 1 }; /* FR thigh */
|
||||
motors[3] = (MotorHandle){ &hcan1, &CAN_1.ID_4_Motor_send, &CAN_1.ID_4_Motor_recieve, 1.0f, 0 }; /* FR shank */
|
||||
motors[4] = (MotorHandle){ &hcan2, &CAN_2.ID_1_Motor_send, &CAN_2.ID_1_Motor_recieve, -1.0f, 1 }; /* RL thigh */
|
||||
motors[5] = (MotorHandle){ &hcan2, &CAN_2.ID_2_Motor_send, &CAN_2.ID_2_Motor_recieve, -1.0f, 0 }; /* RL shank */
|
||||
motors[6] = (MotorHandle){ &hcan2, &CAN_2.ID_3_Motor_send, &CAN_2.ID_3_Motor_recieve, 1.0f, 1 }; /* RR thigh */
|
||||
motors[7] = (MotorHandle){ &hcan2, &CAN_2.ID_4_Motor_send, &CAN_2.ID_4_Motor_recieve, 1.0f, 0 }; /* RR shank */
|
||||
}
|
||||
|
||||
static void motors_enable_all(void)
|
||||
{
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
Motor_Enable(motors[i].hcan, motors[i].tx);
|
||||
osDelay(2);
|
||||
}
|
||||
}
|
||||
|
||||
static void motors_send_control_all(void)
|
||||
{
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
CAN_Send_Control(motors[i].hcan, motors[i].tx);
|
||||
if ((i & 1) == 1) osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void optimize_path(float *target, float current)
|
||||
{
|
||||
float diff = *target - current;
|
||||
if (diff > PI || diff < -PI) {
|
||||
diff = fmodf(diff + PI, 2.0f * PI);
|
||||
if (diff < 0.0f) diff += 2.0f * PI;
|
||||
diff -= PI;
|
||||
*target = current + diff;
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 主任务
|
||||
* ============================================================ */
|
||||
void lingzu_task(const void* argu)
|
||||
{
|
||||
RobotGeometry geom = DEFAULT_GEOMETRY;
|
||||
|
||||
GaitParams gait = {
|
||||
.step_length = 120.0f,
|
||||
.step_height = 35.0f,
|
||||
.period = 0.7f,
|
||||
.start_z = STAND_Z,
|
||||
.duty_cycle = 0.5f,
|
||||
.turn_rate = 0.0f
|
||||
};
|
||||
|
||||
/* ---- 初始化 ---- */
|
||||
osDelay(2000);
|
||||
Lingzu_Motor_Init_Structs();
|
||||
osDelay(100);
|
||||
motors_init_handles();
|
||||
|
||||
motors_enable_all();
|
||||
osDelay(50);
|
||||
motors_enable_all();
|
||||
osDelay(50);
|
||||
|
||||
/* 等待电机上线 */
|
||||
uint32_t init_start = HAL_GetTick();
|
||||
while (HAL_GetTick() - init_start < 5000) {
|
||||
uint32_t now = HAL_GetTick();
|
||||
int all_ready = 1;
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
if (now - motors[i].rx->last_update_time > 200) {
|
||||
Motor_Enable(motors[i].hcan, motors[i].tx);
|
||||
all_ready = 0;
|
||||
osDelay(2);
|
||||
}
|
||||
}
|
||||
if (all_ready) break;
|
||||
osDelay(50);
|
||||
}
|
||||
|
||||
for (int k = 0; k < 20; k++) {
|
||||
motors_send_control_all();
|
||||
osDelay(10);
|
||||
}
|
||||
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
if (now - motors[i].rx->last_update_time > 500)
|
||||
Motor_Enable(motors[i].hcan, motors[i].tx);
|
||||
}
|
||||
}
|
||||
osDelay(100);
|
||||
|
||||
#ifdef CALIBRATE_ZERO_POINT
|
||||
uint32_t cal_start = HAL_GetTick();
|
||||
int zero_done = 0;
|
||||
while(1) {
|
||||
if (!zero_done && (HAL_GetTick() - cal_start > 5000)) {
|
||||
for (int i = 0; i < MOTOR_COUNT; i++)
|
||||
Motor_Zore(motors[i].hcan, motors[i].tx);
|
||||
zero_done = 1;
|
||||
}
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
motors[i].tx->torque = 0; motors[i].tx->kp = 0;
|
||||
motors[i].tx->kd = 0; motors[i].tx->speed = 0;
|
||||
motors[i].tx->position = 0;
|
||||
}
|
||||
motors_send_control_all();
|
||||
osDelay(10);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ---- 偏移量校准（已废弃） ---- */
|
||||
float current_base_z = PRONE_Z;
|
||||
|
||||
{
|
||||
/* 不再自动校准偏移量，完全依赖电机真实的物理零点 */
|
||||
float vmc_offsets[8] = {0};
|
||||
VMC_UpdateEncoderOffsets(vmc_offsets);
|
||||
}
|
||||
|
||||
/* ---- 状态机 ---- */
|
||||
enum { STATE_PRONE = 0, STATE_STAND = 1, STATE_WALK = 2 } robot_state = STATE_PRONE;
|
||||
|
||||
while(1)
|
||||
{
|
||||
/* ======= 0. 后台重连 ======= */
|
||||
static uint32_t last_check = 0;
|
||||
uint32_t now_tick = HAL_GetTick();
|
||||
uint8_t estop = (rc.swD == RC_UP);
|
||||
|
||||
if (!estop && (now_tick - last_check > 500))
|
||||
{
|
||||
last_check = now_tick;
|
||||
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
if (now_tick - motors[i].rx->last_update_time > 500) {
|
||||
Motor_Enable(motors[i].hcan, motors[i].tx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ======= 1. 队列命令 ======= */
|
||||
if (led_control_queue != NULL)
|
||||
{
|
||||
char cmd;
|
||||
if (xQueueReceive(led_control_queue, &cmd, 0) == pdTRUE)
|
||||
{
|
||||
static uint8_t led_st = 0;
|
||||
led_st = !led_st;
|
||||
write_led_io(LED_IO2, led_st ? LED_ON : LED_OFF);
|
||||
|
||||
switch(cmd) {
|
||||
case 'q': if (robot_state != STATE_PRONE) robot_state = STATE_WALK; break;
|
||||
case 's': robot_state = STATE_STAND; break;
|
||||
case 'p': robot_state = STATE_PRONE; break;
|
||||
case 'a': gait.turn_rate = -0.3f; break;
|
||||
case 'd': gait.turn_rate = 0.3f; break;
|
||||
case 'w': gait.turn_rate = 0.0f; break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ======= 2. 高度过渡 ======= */
|
||||
if (robot_state == STATE_WALK || robot_state == STATE_STAND) {
|
||||
if (current_base_z < STAND_Z) {
|
||||
current_base_z += 2.0f;
|
||||
if (current_base_z > STAND_Z) current_base_z = STAND_Z;
|
||||
}
|
||||
} else {
|
||||
if (current_base_z > PRONE_Z) {
|
||||
current_base_z -= 2.0f;
|
||||
if (current_base_z < PRONE_Z) current_base_z = PRONE_Z;
|
||||
}
|
||||
}
|
||||
|
||||
/* ======= 3. 运动控制 ======= */
|
||||
float time_s = (float)HAL_GetTick() / 1000.0f;
|
||||
|
||||
if (robot_state == STATE_PRONE && rc.swC == RC_MI && rc.ch4 > 200)
|
||||
{
|
||||
GaitParams crawl_p = gait;
|
||||
crawl_p.start_z = current_base_z;
|
||||
crawl_p.duty_cycle = 0.25f; /* 匍匐步态应当为真正的 4 节拍爬行, 腾空相占 0.25, 支撑相占 0.75 保证 3 腿支撑 */
|
||||
Quadruped_Crawl(time_s, &crawl_p, &geom);
|
||||
}
|
||||
else if (robot_state == STATE_WALK && current_base_z >= STAND_Z - 5.0f)
|
||||
{
|
||||
GaitParams temp = gait;
|
||||
temp.start_z = current_base_z;
|
||||
int16_t deadzone = 100;
|
||||
|
||||
if (rc.ch2 < -deadzone) {
|
||||
temp.step_length = fabsf(gait.step_length);
|
||||
Quadruped_Forward(time_s, &temp, &geom);
|
||||
} else if (rc.ch2 > deadzone) {
|
||||
Quadruped_Backward(time_s, &temp, &geom);
|
||||
} else if (rc.ch1 < -deadzone) {
|
||||
Quadruped_SpinLeft(time_s, &temp, &geom);
|
||||
} else if (rc.ch1 > deadzone) {
|
||||
Quadruped_SpinRight(time_s, &temp, &geom);
|
||||
} else {
|
||||
Quadruped_InPlace(time_s, &temp, &geom);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float hold_kp = 180.0f, hold_kd = 6.0f;
|
||||
if (robot_state == STATE_PRONE) {
|
||||
Quad_Prone(&geom, current_base_z, hold_kp, hold_kd);
|
||||
} else {
|
||||
Quad_Stand(&geom, current_base_z, hold_kp, hold_kd);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ======= 5. 最短路径优化 ======= */
|
||||
for (int i = 0; i < MOTOR_COUNT; i++) {
|
||||
optimize_path(&motors[i].tx->position, motors[i].rx->current_position_f);
|
||||
}
|
||||
|
||||
/* ======= 6. 急停检查 ======= */
|
||||
if (estop) {
|
||||
osDelay(2);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* ======= 7. 发送控制指令 ======= */
|
||||
motors_send_control_all();
|
||||
osDelay(2);
|
||||
|
||||
/* ======= 8. 心跳 LED ======= */
|
||||
static uint32_t led_tick = 0;
|
||||
if (HAL_GetTick() - led_tick > 500) {
|
||||
led_tick = HAL_GetTick();
|
||||
static uint8_t led3 = 0;
|
||||
led3 = !led3;
|
||||
write_led_io(LED_IO3, led3 ? LED_ON : LED_OFF);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
#ifndef __LINGZU_TASK_H__
|
||||
#define __LINGZU_TASK_H__
|
||||
|
||||
void lingzu_task(const void* argu);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,174 @@
|
||||
#include "uart_task.h"
|
||||
#include "uart_device.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "rm_hal_lib.h"
|
||||
#include "lingzu_motor.h"
|
||||
#include "usart.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
QueueHandle_t led_control_queue;
|
||||
|
||||
static void uart8_send_motor_feedback(uint8_t can_bus, uint8_t motor_id, Motor_CAN_Recieve_Struct *motor)
|
||||
{
|
||||
char msg[160];
|
||||
uint32_t age_ms = HAL_GetTick() - motor->last_update_time;
|
||||
|
||||
int pos_i = (int)motor->current_position_f;
|
||||
int pos_f = (int)(fabs(motor->current_position_f - pos_i) * 10000);
|
||||
int spd_i = (int)motor->current_speed_f;
|
||||
int spd_f = (int)(fabs(motor->current_speed_f - spd_i) * 10000);
|
||||
int tor_i = (int)motor->current_torque_f;
|
||||
int tor_f = (int)(fabs(motor->current_torque_f - tor_i) * 10000);
|
||||
int tmp_i = (int)motor->current_temp_f;
|
||||
int tmp_f = (int)(fabs(motor->current_temp_f - tmp_i) * 10);
|
||||
|
||||
int len = snprintf(msg, sizeof(msg),
|
||||
"CAN%u,ID%u,POS:%s%d.%04d,SPD:%s%d.%04d,TOR:%s%d.%04d,TMP:%s%d.%01d,FAULT:0x%02X,AGE:%lu\r\n",
|
||||
can_bus,
|
||||
motor_id,
|
||||
(motor->current_position_f < 0 && pos_i == 0) ? "-" : "", pos_i, pos_f,
|
||||
(motor->current_speed_f < 0 && spd_i == 0) ? "-" : "", spd_i, spd_f,
|
||||
(motor->current_torque_f < 0 && tor_i == 0) ? "-" : "", tor_i, tor_f,
|
||||
(motor->current_temp_f < 0 && tmp_i == 0) ? "-" : "", tmp_i, tmp_f,
|
||||
motor->fault_message,
|
||||
(unsigned long)age_ms);
|
||||
|
||||
if (len > 0)
|
||||
{
|
||||
HAL_UART_Transmit(&huart8, (uint8_t *)msg, (uint16_t)len, 20);
|
||||
}
|
||||
}
|
||||
|
||||
static void uart8_send_all_motor_feedback(void)
|
||||
{
|
||||
uart8_send_motor_feedback(1, 1, &CAN_1.ID_1_Motor_recieve);
|
||||
uart8_send_motor_feedback(1, 2, &CAN_1.ID_2_Motor_recieve);
|
||||
uart8_send_motor_feedback(1, 3, &CAN_1.ID_3_Motor_recieve);
|
||||
uart8_send_motor_feedback(1, 4, &CAN_1.ID_4_Motor_recieve);
|
||||
uart8_send_motor_feedback(2, 1, &CAN_2.ID_1_Motor_recieve);
|
||||
uart8_send_motor_feedback(2, 2, &CAN_2.ID_2_Motor_recieve);
|
||||
uart8_send_motor_feedback(2, 3, &CAN_2.ID_3_Motor_recieve);
|
||||
uart8_send_motor_feedback(2, 4, &CAN_2.ID_4_Motor_recieve);
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 根据遥控器 SwD 开关状态控制灵足电机使能/失能
|
||||
* 约定: SwD 上=软急停(失能全部电机), SwD 下=恢复行走(使能全部电机)
|
||||
*/
|
||||
static void RC_SwD_Motor_StartStop_Update(void)
|
||||
{
|
||||
static uint8_t last_swD = 0;
|
||||
|
||||
if (rc.swD == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (rc.swD != last_swD)
|
||||
{
|
||||
if (rc.swD == RC_UP)
|
||||
{
|
||||
LINGZU_All_Motors_Limp(); // User/driver/lingzu_motor.c
|
||||
DISABLE_ALL_LINGZU_MOTORS(); // User/driver/lingzu_motor.c
|
||||
}
|
||||
else if (rc.swD == RC_DN)
|
||||
{
|
||||
ENABLE_ALL_LINGZU_MOTORS(); // User/driver/lingzu_motor.c
|
||||
}
|
||||
last_swD = rc.swD;
|
||||
}
|
||||
}
|
||||
|
||||
void uart_task(const void* argu)
|
||||
{
|
||||
/* 创建队列，深度为10，每个单元大小为char */
|
||||
led_control_queue = xQueueCreate(10, sizeof(char));
|
||||
|
||||
while(1)
|
||||
{
|
||||
/*
|
||||
* SwC (rc.sw3) 控制 站立/趴下
|
||||
* RC_DN (2): 站立 (Stand) -> 发送 's'
|
||||
* RC_UP (1) / RC_MI (3): 趴下 (Prone) -> 发送 'p'
|
||||
*
|
||||
* 调试信息：如果 rc.sw3 一直是 0，说明遥控器没连上或没解析到数据。
|
||||
* 为了验证是否接收到数据，如果接收到任何非零数据，闪烁 LED1。
|
||||
*/
|
||||
|
||||
// 强制闪烁逻辑：只要进入了 while(1) 循环，LED1 就以 1Hz 闪烁
|
||||
// 如果遥控器有数据，则改为快闪
|
||||
static int debug_cnt = 0;
|
||||
debug_cnt++;
|
||||
|
||||
// 只要任一通道有数据，就认为连接正常
|
||||
// 简化逻辑：只检查 SwA (十通) 是否有数据变化，或者检查基本通道
|
||||
// 这里我们检查 SwA 是否非0 (说明已解析)，或者 ch1 (右摇杆) 是否有值
|
||||
// 为了稳健，只要 rc 结构体非全0即可。这里沿用之前的风格，检查 SwA。
|
||||
if (rc.swA != 0) {
|
||||
// 接收到有效遥控数据
|
||||
// 调试逻辑：如果 SwA 处于“下”位 (RC_DN)，则常亮 LED1
|
||||
// 这样用户可以测试 SwA 是否对应“站立”指令
|
||||
if (rc.swA == RC_DN) {
|
||||
write_led_io(LED_IO1, LED_ON);
|
||||
} else {
|
||||
// 否则快闪，表示连接正常但处于趴下模式 (SwA 在上或中)
|
||||
write_led_io(LED_IO1, (debug_cnt % 2) ? LED_ON : LED_OFF);
|
||||
}
|
||||
} else {
|
||||
// 未接收到数据：慢闪
|
||||
write_led_io(LED_IO1, (debug_cnt / 25) % 2 ? LED_ON : LED_OFF);
|
||||
}
|
||||
|
||||
static char last_sent_cmd = 0;
|
||||
char current_cmd = 'p';
|
||||
|
||||
// 逻辑：仅使用 SwA (十通) 控制
|
||||
// SwA 下 (RC_DN) -> 站立/行走
|
||||
// SwA 上/中 -> 趴下
|
||||
if (rc.swA == RC_DN)
|
||||
{
|
||||
if(rc.ch4 > 200)
|
||||
{
|
||||
current_cmd = 'q'; // Walk
|
||||
}
|
||||
else
|
||||
{
|
||||
current_cmd = 's'; // Stand (Stop walking)
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
current_cmd = 'p'; // Prone
|
||||
}
|
||||
|
||||
// 仅在状态改变时发送命令，防止队列溢出
|
||||
// 添加超时重发机制 (每1秒重发一次) 以防止丢包
|
||||
static int resend_cnt = 0;
|
||||
resend_cnt++;
|
||||
|
||||
if (current_cmd != last_sent_cmd || resend_cnt > 50)
|
||||
{
|
||||
xQueueSend(led_control_queue, ¤t_cmd, 0);
|
||||
last_sent_cmd = current_cmd;
|
||||
resend_cnt = 0;
|
||||
}
|
||||
|
||||
// 根据 SwD 状态控制电机启停
|
||||
RC_SwD_Motor_StartStop_Update();
|
||||
|
||||
static uint32_t last_uart8_send_tick = 0;
|
||||
uint32_t now_tick = HAL_GetTick();
|
||||
if (now_tick - last_uart8_send_tick >= 100)
|
||||
{
|
||||
last_uart8_send_tick = now_tick;
|
||||
uart8_send_all_motor_feedback();
|
||||
}
|
||||
|
||||
/* 延时 20ms，控制发送频率 */
|
||||
osDelay(20);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
#ifndef __UART_TASK_H__
|
||||
#define __UART_TASK_H__
|
||||
|
||||
#include "FreeRTOS.h"
|
||||
#include "queue.h"
|
||||
|
||||
extern QueueHandle_t led_control_queue;
|
||||
|
||||
void uart_task(const void* argu);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,370 @@
|
||||
/**
|
||||
* @file Movement.c
|
||||
* @brief 四足运动控制 — 直接电机角映射版
|
||||
*
|
||||
* 改动:
|
||||
* - IK 直接输出无符号电机角, 不再经过 PostureConfig
|
||||
* - FK/Jacobian 基于实测标定参数 (foot_offset, thigh_zero, shank_coupling_c)
|
||||
* - use_full_vmc=0 时暂时关闭全状态VMC, 先验证IK
|
||||
*/
|
||||
|
||||
#include "Movement.h"
|
||||
#include <math.h>
|
||||
|
||||
/* ============================================================
|
||||
* 编码器偏置 (从 lingzu_task.c 同步)
|
||||
* ============================================================ */
|
||||
static float vmc_enc_offset[LEG_COUNT][2] = {{0},{0},{0},{0}};
|
||||
static uint8_t vmc_offsets_valid = 0;
|
||||
|
||||
void VMC_UpdateEncoderOffsets(const float offsets[8])
|
||||
{
|
||||
vmc_enc_offset[LEG_FL][0] = offsets[0];
|
||||
vmc_enc_offset[LEG_FL][1] = offsets[1];
|
||||
vmc_enc_offset[LEG_FR][0] = offsets[2];
|
||||
vmc_enc_offset[LEG_FR][1] = offsets[3];
|
||||
vmc_enc_offset[LEG_RL][0] = offsets[4];
|
||||
vmc_enc_offset[LEG_RL][1] = offsets[5];
|
||||
vmc_enc_offset[LEG_RR][0] = offsets[6];
|
||||
vmc_enc_offset[LEG_RR][1] = offsets[7];
|
||||
vmc_offsets_valid = 1;
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 底层
|
||||
* ============================================================ */
|
||||
void Posture(int motor_id, float position, float speed,
|
||||
float kp, float kd, float torque)
|
||||
{
|
||||
rs02_set_target_rad((uint8_t)motor_id, position, speed, kp, kd, torque);
|
||||
}
|
||||
|
||||
void Leg_Thigh_SetTarget(leg_index_e leg, float q, float speed,
|
||||
float kp, float kd, float torque)
|
||||
{
|
||||
if (leg >= LEG_COUNT) return;
|
||||
Posture(LEG_MOTOR_MAP[leg][0], q, speed, kp, kd, torque);
|
||||
}
|
||||
|
||||
void Leg_Shank_SetTarget(leg_index_e leg, float q, float speed,
|
||||
float kp, float kd, float torque)
|
||||
{
|
||||
if (leg >= LEG_COUNT) return;
|
||||
Posture(LEG_MOTOR_MAP[leg][1], q, speed, kp, kd, torque);
|
||||
}
|
||||
|
||||
void Leg_All_SetTarget(leg_index_e leg, float q1, float q2, float speed,
|
||||
float kp, float kd, float torque)
|
||||
{
|
||||
Leg_Thigh_SetTarget(leg, q1, speed, kp, kd, torque);
|
||||
Leg_Shank_SetTarget(leg, q2, speed, kp, kd, torque);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 工具
|
||||
* ============================================================ */
|
||||
static float clampf(float v, float lo, float hi)
|
||||
{
|
||||
if (v < lo) return lo;
|
||||
if (v > hi) return hi;
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* VMC 力矩 — 旧版简化 (开环, 轨迹目标位置)
|
||||
* ============================================================ */
|
||||
static void vmc_legacy_torque(float mt_unsigned, float ms_unsigned,
|
||||
float x_ref_mm, float z_ref_mm,
|
||||
const RobotGeometry *geom,
|
||||
const VmcConfig *cfg,
|
||||
float *tau1, float *tau2)
|
||||
{
|
||||
*tau1 = 0.0f; *tau2 = 0.0f;
|
||||
if (!cfg->enable) return;
|
||||
|
||||
/* FK 计算轨迹足端位置 */
|
||||
float x_traj, z_traj;
|
||||
FK_LegPosition(mt_unsigned, ms_unsigned, geom, &x_traj, &z_traj);
|
||||
|
||||
float fx = -cfg->kx * (x_traj - x_ref_mm) * 0.001f;
|
||||
float fz = cfg->kz * (z_ref_mm - z_traj) * 0.001f;
|
||||
|
||||
/* Jacobian 转置 (基于实际角度) */
|
||||
float foff = geom->foot_offset_rad;
|
||||
float theta_hip = -(mt_unsigned + geom->thigh_zero);
|
||||
float theta_sl = -(ms_unsigned + mt_unsigned) + geom->shank_coupling_c;
|
||||
float phi = theta_sl - foff;
|
||||
float cos_splay = cosf(geom->splay_angle_rad);
|
||||
|
||||
float L1m = geom->L1 * 0.001f;
|
||||
float L2m = geom->L2 * 0.001f;
|
||||
|
||||
/* dx/dmt, dz/dmt 等 (见 leg.c 注释) */
|
||||
float Jx_mt = -(L1m * cosf(theta_hip) + L2m * cosf(phi));
|
||||
float Jx_ms = -(L2m * cosf(phi));
|
||||
float Jz_mt = (L1m * sinf(theta_hip) + L2m * sinf(phi)) * cos_splay;
|
||||
float Jz_ms = (L2m * sinf(phi)) * cos_splay;
|
||||
|
||||
/* J^T * F → τ */
|
||||
*tau1 = clampf(Jx_mt * fx + Jz_mt * fz, -cfg->torque_limit, cfg->torque_limit);
|
||||
*tau2 = clampf(Jx_ms * fx + Jz_ms * fz, -cfg->torque_limit, cfg->torque_limit);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* VMC 力矩 — 全状态闭环 (编码器反馈)
|
||||
* ============================================================ */
|
||||
static void vmc_full_torque(leg_index_e leg,
|
||||
float x_ref_mm, float z_ref_mm,
|
||||
const RobotGeometry *geom,
|
||||
const VmcFullConfig *cfg,
|
||||
float *tau1, float *tau2)
|
||||
{
|
||||
*tau1 = 0.0f; *tau2 = 0.0f;
|
||||
if (!cfg->enable || !vmc_offsets_valid) return;
|
||||
|
||||
float sign = SIDE_SIGNS[leg];
|
||||
int tid = LEG_MOTOR_MAP[leg][0];
|
||||
int sid = LEG_MOTOR_MAP[leg][1];
|
||||
|
||||
/* 读取编码器, 去偏置, 去镜像 → 无符号电机角 */
|
||||
float mt_raw = rs02_get_position_rad((uint8_t)tid);
|
||||
float ms_raw = rs02_get_position_rad((uint8_t)sid);
|
||||
float mt = (mt_raw - vmc_enc_offset[leg][0]) / sign;
|
||||
float ms = (ms_raw - vmc_enc_offset[leg][1]) / sign;
|
||||
|
||||
/* FK: 实际足端位置 */
|
||||
float x_act, z_act;
|
||||
FK_LegPosition(mt, ms, geom, &x_act, &z_act);
|
||||
|
||||
/* 速度 */
|
||||
float vt_raw = rs02_get_velocity_rad((uint8_t)tid);
|
||||
float vs_raw = rs02_get_velocity_rad((uint8_t)sid);
|
||||
float vt = vt_raw / sign;
|
||||
float vs = vs_raw / sign;
|
||||
|
||||
float xdot, zdot;
|
||||
FK_LegVelocity(mt, ms, vt, vs, geom, &xdot, &zdot);
|
||||
|
||||
/* 虚拟力 */
|
||||
float fx = -cfg->kx * (x_act - x_ref_mm) * 0.001f - cfg->bx * xdot * 0.001f;
|
||||
float fz = -cfg->kz * (z_act - z_ref_mm) * 0.001f - cfg->bz * zdot * 0.001f + cfg->gravity_ff;
|
||||
|
||||
/* Jacobian (基于实际角度) */
|
||||
float foff = geom->foot_offset_rad;
|
||||
float theta_hip = -(mt + geom->thigh_zero);
|
||||
float theta_sl = -(ms + mt) + geom->shank_coupling_c;
|
||||
float phi = theta_sl - foff;
|
||||
float cos_splay = cosf(geom->splay_angle_rad);
|
||||
|
||||
float L1m = geom->L1 * 0.001f;
|
||||
float L2m = geom->L2 * 0.001f;
|
||||
|
||||
float Jx_mt = -(L1m * cosf(theta_hip) + L2m * cosf(phi));
|
||||
float Jx_ms = -(L2m * cosf(phi));
|
||||
float Jz_mt = (L1m * sinf(theta_hip) + L2m * sinf(phi)) * cos_splay;
|
||||
float Jz_ms = (L2m * sinf(phi)) * cos_splay;
|
||||
|
||||
*tau1 = clampf(Jx_mt * fx + Jz_mt * fz, -cfg->torque_limit, cfg->torque_limit);
|
||||
*tau2 = clampf(Jx_ms * fx + Jz_ms * fz, -cfg->torque_limit, cfg->torque_limit);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 静态姿态
|
||||
* ============================================================ */
|
||||
void Quad_Hold(const RobotGeometry *geom, float base_z, float kp, float kd)
|
||||
{
|
||||
float ratio = (base_z - PRONE_Z) / (STAND_Z - PRONE_Z + 0.1f);
|
||||
if (ratio < 0.0f) ratio = 0.0f;
|
||||
if (ratio > 1.0f) ratio = 1.0f;
|
||||
|
||||
for (int i = 0; i < LEG_COUNT; i++)
|
||||
{
|
||||
float target_x = LEG_STANCE_X_OFFSET[i] * ratio;
|
||||
float mt, ms;
|
||||
Inverse_Calculation(target_x, base_z, &mt, &ms, geom);
|
||||
|
||||
float q1 = mt * SIDE_SIGNS[i];
|
||||
float q2 = ms * SIDE_SIGNS[i];
|
||||
Leg_All_SetTarget((leg_index_e)i, q1, q2, 0.0f, kp, kd, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
void Quad_Prone(const RobotGeometry *geom, float base_z, float kp, float kd)
|
||||
{
|
||||
Quad_Hold(geom, base_z, kp, kd);
|
||||
}
|
||||
|
||||
void Quad_Stand(const RobotGeometry *geom, float base_z, float kp, float kd)
|
||||
{
|
||||
Quad_Hold(geom, base_z, kp, kd);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* Trot 步态 — 核心
|
||||
* ============================================================ */
|
||||
void Quadruped_Trot(float t, const GaitParams *params,
|
||||
const RobotGeometry *geom, const GaitProfile *profile)
|
||||
{
|
||||
float period = params->period;
|
||||
float start_z = params->start_z;
|
||||
float step_len = params->step_length;
|
||||
float step_h = fabsf(params->step_height);
|
||||
float turn_rate = params->turn_rate;
|
||||
float swing_ratio = params->duty_cycle;
|
||||
if (swing_ratio < 0.1f || swing_ratio > 0.9f) swing_ratio = 0.5f;
|
||||
|
||||
float left_step = step_len * (1.0f + turn_rate);
|
||||
float right_step = step_len * (1.0f - turn_rate);
|
||||
if (fabsf(step_len) < 0.01f) {
|
||||
/* 当基础步长为0时，使用 turn_rate 直接作为纯自旋步长差 (mm) */
|
||||
left_step = turn_rate;
|
||||
right_step = -turn_rate;
|
||||
}
|
||||
|
||||
float ratio = (start_z - PRONE_Z) / (STAND_Z - PRONE_Z + 0.1f);
|
||||
if (ratio < 0.0f) ratio = 0.0f;
|
||||
if (ratio > 1.0f) ratio = 1.0f;
|
||||
|
||||
for (int i = 0; i < LEG_COUNT; i++)
|
||||
{
|
||||
leg_index_e leg = (leg_index_e)i;
|
||||
float leg_step = (i == LEG_FL || i == LEG_RL) ? left_step : right_step;
|
||||
float half = leg_step / 2.0f;
|
||||
|
||||
float current_phase = (t / period) + TROT_PHASES[i];
|
||||
float norm_phase = fmodf(current_phase, 1.0f);
|
||||
if (norm_phase < 0.0f) norm_phase += 1.0f;
|
||||
|
||||
float x_traj, z_traj;
|
||||
Gen_Bezier_Trajectory(current_phase, swing_ratio,
|
||||
-half, half, start_z, step_h,
|
||||
&x_traj, &z_traj);
|
||||
|
||||
/* 增加 X 轴静步态偏置 */
|
||||
x_traj += LEG_STANCE_X_OFFSET[i] * ratio;
|
||||
|
||||
int is_swing = (norm_phase < swing_ratio);
|
||||
|
||||
/* IK: 直接得到无符号电机角 */
|
||||
float mt, ms;
|
||||
Inverse_Calculation(x_traj, z_traj, &mt, &ms, geom);
|
||||
|
||||
/* 左右镜像 */
|
||||
float target_q1 = mt * SIDE_SIGNS[i];
|
||||
float target_q2 = ms * SIDE_SIGNS[i];
|
||||
|
||||
/* PD 参数 */
|
||||
const ControlGains *gains = is_swing
|
||||
? &profile->swing_gains
|
||||
: &profile->stance_gains;
|
||||
|
||||
/* VMC 力矩 */
|
||||
float tau1 = 0.0f, tau2 = 0.0f;
|
||||
if (profile->use_full_vmc && vmc_offsets_valid) {
|
||||
const VmcFullConfig *vcfg = is_swing
|
||||
? &profile->vmc_swing
|
||||
: &profile->vmc_stance;
|
||||
vmc_full_torque(leg, x_traj, z_traj, geom, vcfg, &tau1, &tau2);
|
||||
} else {
|
||||
vmc_legacy_torque(mt, ms, x_traj, z_traj, geom,
|
||||
&profile->vmc, &tau1, &tau2);
|
||||
}
|
||||
|
||||
float tlimit = profile->use_full_vmc
|
||||
? (is_swing ? profile->vmc_swing.torque_limit
|
||||
: profile->vmc_stance.torque_limit)
|
||||
: profile->vmc.torque_limit;
|
||||
|
||||
float tq1 = clampf(gains->torque + tau1, -tlimit, tlimit);
|
||||
float tq2 = clampf(gains->torque + tau2, -tlimit, tlimit);
|
||||
|
||||
Posture(LEG_MOTOR_MAP[i][0], target_q1, 0.0f,
|
||||
gains->kp_thigh, gains->kd, tq1);
|
||||
Posture(LEG_MOTOR_MAP[i][1], target_q2, 0.0f,
|
||||
gains->kp_shank, gains->kd, tq2);
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 便捷接口
|
||||
* ============================================================ */
|
||||
void Quadruped_Forward(float t, const GaitParams *params, const RobotGeometry *geom)
|
||||
{
|
||||
Quadruped_Trot(t, params, geom, &PROFILE_TROT_FORWARD);
|
||||
}
|
||||
|
||||
void Quadruped_Backward(float t, const GaitParams *params, const RobotGeometry *geom)
|
||||
{
|
||||
GaitParams p = *params;
|
||||
p.step_length = -50.0f; /* 必须为负数才能向后运动 */
|
||||
p.turn_rate = 0.0f;
|
||||
Quadruped_Trot(t, &p, geom, &PROFILE_TROT_WALK);
|
||||
}
|
||||
|
||||
void Quadruped_InPlace(float t, const GaitParams *params, const RobotGeometry *geom)
|
||||
{
|
||||
GaitParams p = *params;
|
||||
p.step_length = 0.0f;
|
||||
p.turn_rate = 0.0f;
|
||||
Quadruped_Trot(t, &p, geom, &PROFILE_TROT_INPLACE);
|
||||
}
|
||||
|
||||
void Quadruped_SpinLeft(float t, const GaitParams *params, const RobotGeometry *geom)
|
||||
{
|
||||
GaitParams p = *params;
|
||||
p.step_length = 0.0f;
|
||||
p.turn_rate = -100.0f; /* 左转：左侧倒退，右侧前进 */
|
||||
Quadruped_Trot(t, &p, geom, &PROFILE_TROT_INPLACE);
|
||||
}
|
||||
|
||||
void Quadruped_SpinRight(float t, const GaitParams *params, const RobotGeometry *geom)
|
||||
{
|
||||
GaitParams p = *params;
|
||||
p.step_length = 0.0f;
|
||||
p.turn_rate = 100.0f; /* 右转：左侧前进，右侧倒退 */
|
||||
Quadruped_Trot(t, &p, geom, &PROFILE_TROT_INPLACE);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 爬行步态
|
||||
* ============================================================ */
|
||||
void Quadruped_Crawl(float t, const GaitParams *params, const RobotGeometry *geom)
|
||||
{
|
||||
float period = params->period;
|
||||
float start_z = params->start_z;
|
||||
float step_len = params->step_length;
|
||||
float step_h = fabsf(params->step_height);
|
||||
float swing_ratio = params->duty_cycle;
|
||||
if (swing_ratio < 0.1f || swing_ratio > 0.9f) swing_ratio = 0.5f;
|
||||
|
||||
float ratio = (start_z - PRONE_Z) / (STAND_Z - PRONE_Z + 0.1f);
|
||||
if (ratio < 0.0f) ratio = 0.0f;
|
||||
if (ratio > 1.0f) ratio = 1.0f;
|
||||
|
||||
for (int i = 0; i < LEG_COUNT; i++)
|
||||
{
|
||||
float current_phase = (t / period) + CRAWL_PHASES[i];
|
||||
float norm_phase = fmodf(current_phase, 1.0f);
|
||||
if (norm_phase < 0.0f) norm_phase += 1.0f;
|
||||
|
||||
float half = step_len / 2.0f;
|
||||
float x_traj, z_traj;
|
||||
Gen_Bezier_Trajectory(current_phase, swing_ratio,
|
||||
-half, half, start_z, step_h,
|
||||
&x_traj, &z_traj);
|
||||
|
||||
x_traj += LEG_STANCE_X_OFFSET[i] * ratio;
|
||||
|
||||
const ControlGains *gains = (norm_phase < swing_ratio)
|
||||
? &CRAWL_SWING_GAINS
|
||||
: &CRAWL_STANCE_GAINS;
|
||||
|
||||
float mt, ms;
|
||||
Inverse_Calculation(x_traj, z_traj, &mt, &ms, geom);
|
||||
|
||||
Posture(LEG_MOTOR_MAP[i][0], mt * SIDE_SIGNS[i], 0.0f,
|
||||
gains->kp_thigh, gains->kd, gains->torque);
|
||||
Posture(LEG_MOTOR_MAP[i][1], ms * SIDE_SIGNS[i], 0.0f,
|
||||
gains->kp_shank, gains->kd, gains->torque);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
#ifndef __MOVEMENT_H__
|
||||
#define __MOVEMENT_H__
|
||||
|
||||
#include "gait_config.h"
|
||||
#include "leg.h"
|
||||
#include "lingzu_motor.h"
|
||||
|
||||
/* ============================================================
|
||||
* 底层: 单电机指令
|
||||
* ============================================================ */
|
||||
void Posture(int motor_id, float position, float speed, float kp, float kd, float torque);
|
||||
|
||||
/* ============================================================
|
||||
* 单腿控制
|
||||
* ============================================================ */
|
||||
void Leg_Thigh_SetTarget(leg_index_e leg, float q, float speed, float kp, float kd, float torque);
|
||||
void Leg_Shank_SetTarget(leg_index_e leg, float q, float speed, float kp, float kd, float torque);
|
||||
void Leg_All_SetTarget(leg_index_e leg, float q1, float q2, float speed, float kp, float kd, float torque);
|
||||
|
||||
/* ============================================================
|
||||
* VMC 编码器偏置同步
|
||||
* ============================================================ */
|
||||
void VMC_UpdateEncoderOffsets(const float offsets[8]);
|
||||
|
||||
/* ============================================================
|
||||
* 静态姿态
|
||||
* ============================================================ */
|
||||
void Quad_Hold(const RobotGeometry *geom, float base_z, float kp, float kd);
|
||||
void Quad_Prone(const RobotGeometry *geom, float base_z, float kp, float kd);
|
||||
void Quad_Stand(const RobotGeometry *geom, float base_z, float kp, float kd);
|
||||
|
||||
/* ============================================================
|
||||
* Trot 步态
|
||||
* ============================================================ */
|
||||
void Quadruped_Trot(float t, const GaitParams *params,
|
||||
const RobotGeometry *geom, const GaitProfile *profile);
|
||||
|
||||
void Quadruped_Forward(float t, const GaitParams *params, const RobotGeometry *geom);
|
||||
void Quadruped_Backward(float t, const GaitParams *params, const RobotGeometry *geom);
|
||||
void Quadruped_InPlace(float t, const GaitParams *params, const RobotGeometry *geom);
|
||||
void Quadruped_SpinLeft(float t, const GaitParams *params, const RobotGeometry *geom);
|
||||
void Quadruped_SpinRight(float t, const GaitParams *params, const RobotGeometry *geom);
|
||||
|
||||
/* ============================================================
|
||||
* 爬行步态
|
||||
* ============================================================ */
|
||||
void Quadruped_Crawl(float t, const GaitParams *params, const RobotGeometry *geom);
|
||||
|
||||
#endif /* __MOVEMENT_H__ */
|
||||
@@ -0,0 +1,168 @@
|
||||
#include "gait_config.h"
|
||||
|
||||
/* ============================================================
|
||||
* 默认几何参数 (实测硬件)
|
||||
* ============================================================ */
|
||||
const RobotGeometry DEFAULT_GEOMETRY = {
|
||||
.L1 = 250.0f,
|
||||
.L2 = 290.0f,
|
||||
.foot_offset_rad = FOOT_OFFSET_RAD,
|
||||
.thigh_zero = THIGH_ZERO_RAD,
|
||||
.shank_coupling_c = SHANK_COUPLING_C,
|
||||
.splay_angle_rad = LEG_SPLAY_ANGLE_RAD
|
||||
};
|
||||
|
||||
/* ===========================FOOT_OFFSET_RAD=================================
|
||||
* 步态配置集 — 前进 Trot
|
||||
* ============================================================ */
|
||||
const GaitProfile PROFILE_TROT_FORWARD = {
|
||||
.swing_gains = {
|
||||
.kp_thigh = 220.0f,
|
||||
.kp_shank = 210.0f,
|
||||
.kd = 4.0f,
|
||||
.torque = 5.5f
|
||||
},
|
||||
.stance_gains = {
|
||||
.kp_thigh = 220.0f,
|
||||
.kp_shank = 210.0f,
|
||||
.kd = 4.0f,
|
||||
.torque = 10.0f
|
||||
},
|
||||
.vmc = {
|
||||
.enable = 1,
|
||||
.kx = 35.0f,
|
||||
.kz = 55.0f,
|
||||
.torque_limit = 8.0f
|
||||
},
|
||||
.vmc_stance = {
|
||||
.enable = 1,
|
||||
.kx = 50.0f,
|
||||
.kz = 300.0f,
|
||||
.bx = 8.0f,
|
||||
.bz = 15.0f,
|
||||
.gravity_ff = 0.0f,
|
||||
.torque_limit = 12.0f
|
||||
},
|
||||
.vmc_swing = {
|
||||
.enable = 1,
|
||||
.kx = 0.0f,
|
||||
.kz = 0.0f,
|
||||
.bx = 2.0f,
|
||||
.bz = 2.0f,
|
||||
.gravity_ff = 0.0f,
|
||||
.torque_limit = 4.0f
|
||||
},
|
||||
.use_full_vmc = 0 /* 先关闭, 验证IK正确后再开 */
|
||||
};
|
||||
|
||||
/* ============================================================
|
||||
* 步态配置集 — 通用行走
|
||||
* ============================================================ */
|
||||
const GaitProfile PROFILE_TROT_WALK = {
|
||||
.swing_gains = {
|
||||
.kp_thigh = 220.0f,
|
||||
.kp_shank = 210.0f,
|
||||
.kd = 3.0f,
|
||||
.torque = 5.0f
|
||||
},
|
||||
.stance_gains = {
|
||||
.kp_thigh = 220.0f,
|
||||
.kp_shank = 210.0f,
|
||||
.kd = 3.0f,
|
||||
.torque = 5.0f
|
||||
},
|
||||
.vmc = {
|
||||
.enable = 1,
|
||||
.kx = 35.0f,
|
||||
.kz = 55.0f,
|
||||
.torque_limit = 8.0f
|
||||
},
|
||||
.vmc_stance = {
|
||||
.enable = 1,
|
||||
.kx = 50.0f,
|
||||
.kz = 300.0f,
|
||||
.bx = 8.0f,
|
||||
.bz = 15.0f,
|
||||
.gravity_ff = 0.0f,
|
||||
.torque_limit = 10.0f
|
||||
},
|
||||
.vmc_swing = {
|
||||
.enable = 1,
|
||||
.kx = 0.0f,
|
||||
.kz = 0.0f,
|
||||
.bx = 2.0f,
|
||||
.bz = 2.0f,
|
||||
.gravity_ff = 0.0f,
|
||||
.torque_limit = 4.0f
|
||||
},
|
||||
.use_full_vmc = 0
|
||||
};
|
||||
|
||||
/* ============================================================
|
||||
* 步态配置集 — 原地踏步/转向
|
||||
* ============================================================ */
|
||||
const GaitProfile PROFILE_TROT_INPLACE = {
|
||||
.swing_gains = {
|
||||
.kp_thigh = 120.0f,
|
||||
.kp_shank = 130.0f,
|
||||
.kd = 15.0f,
|
||||
.torque = 5.0f
|
||||
},
|
||||
.stance_gains = {
|
||||
.kp_thigh = 140.0f,
|
||||
.kp_shank = 150.0f,
|
||||
.kd = 15.0f,
|
||||
.torque = 7.0f
|
||||
},
|
||||
.vmc = {
|
||||
.enable = 1,
|
||||
.kx = 35.0f,
|
||||
.kz = 55.0f,
|
||||
.torque_limit = 8.0f
|
||||
},
|
||||
.vmc_stance = {
|
||||
.enable = 1,
|
||||
.kx = 60.0f,
|
||||
.kz = 300.0f,
|
||||
.bx = 10.0f,
|
||||
.bz = 15.0f,
|
||||
.gravity_ff = 0.0f,
|
||||
.torque_limit = 12.0f
|
||||
},
|
||||
.vmc_swing = {
|
||||
.enable = 1,
|
||||
.kx = 0.0f,
|
||||
.kz = 0.0f,
|
||||
.bx = 2.0f,
|
||||
.bz = 2.0f,
|
||||
.gravity_ff = 0.0f,
|
||||
.torque_limit = 4.0f
|
||||
},
|
||||
.use_full_vmc = 0
|
||||
};
|
||||
|
||||
/* ============================================================
|
||||
* 爬行模式 PD 参数
|
||||
* ============================================================ */
|
||||
const ControlGains CRAWL_SWING_GAINS = {
|
||||
.kp_thigh = 45.0f,
|
||||
.kp_shank = 45.0f,
|
||||
.kd = 8.0f,
|
||||
.torque = 2.0f
|
||||
};
|
||||
|
||||
const ControlGains CRAWL_STANCE_GAINS = {
|
||||
.kp_thigh = 40.0f,
|
||||
.kp_shank = 40.0f,
|
||||
.kd = 8.0f,
|
||||
.torque = 6.0f
|
||||
};
|
||||
|
||||
/* 腿部站立初始的 X 轴偏置，可用于改善因为重心靠后导致后腿受力过大的问题 */
|
||||
// 前腿稍微往前一点 ，后腿根据需求"靠后一点"给出明显的负数偏置
|
||||
const float LEG_STANCE_X_OFFSET[LEG_COUNT] = {
|
||||
10.0f, /* FL */
|
||||
10.0f, /* FR */
|
||||
-65.0f, /* RL: 减小负数偏置，以前是*/
|
||||
-65.0f /* RR */
|
||||
};
|
||||
@@ -0,0 +1,157 @@
|
||||
#ifndef __GAIT_CONFIG_H__
|
||||
#define __GAIT_CONFIG_H__
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.1415926535f
|
||||
#endif
|
||||
#define DEG2RAD(x) ((x) * PI / 180.0f)
|
||||
|
||||
/* ============================================================
|
||||
* 腿部索引 & 镜像
|
||||
* ============================================================ */
|
||||
typedef enum {
|
||||
LEG_FL = 0,
|
||||
LEG_FR = 1,
|
||||
LEG_RL = 2,
|
||||
LEG_RR = 3,
|
||||
LEG_COUNT = 4
|
||||
} leg_index_e;
|
||||
|
||||
/* 左右镜像: FL/RL = -1, FR/RR = +1 */
|
||||
static const float SIDE_SIGNS[LEG_COUNT] = {-1.0f, 1.0f, -1.0f, 1.0f};
|
||||
|
||||
/* Trot 对角腿同相 */
|
||||
static const float TROT_PHASES[LEG_COUNT] = {0.0f, 0.5f, 0.5f, 0.0f};
|
||||
|
||||
/* Crawl 相位: 依次迈腿 (四拍步态) */
|
||||
static const float CRAWL_PHASES[LEG_COUNT] = {0.0f, 0.5f, 0.75f, 0.25f};
|
||||
|
||||
/* 电机映射: [leg][0]=大腿, [leg][1]=小腿 */
|
||||
static const int LEG_MOTOR_MAP[LEG_COUNT][2] = {
|
||||
{1, 2}, /* FL: CAN1 ID1, ID2 */
|
||||
{3, 4}, /* FR: CAN1 ID3, ID4 */
|
||||
{5, 6}, /* RL: CAN2 ID1, ID2 */
|
||||
{7, 8} /* RR: CAN2 ID3, ID4 */
|
||||
};
|
||||
|
||||
/* ============================================================
|
||||
* 机器人几何参数 (硬件实测)
|
||||
*
|
||||
* 坐标系: 原点=髋关节, X前向正, Z向下正
|
||||
* 电机零位: 趴下时所有电机读数=0
|
||||
*
|
||||
* 硬件标定数据 (右侧无符号):
|
||||
* 大腿垂直时 motor = -1.0 rad
|
||||
* 小腿链节垂直时 motor = +2.76 rad
|
||||
* 足端安装偏置 = 40° (小腿前倾40°足端才垂直)
|
||||
*
|
||||
* 同步带 1:1 耦合模型:
|
||||
* θ_hip = -(motor_thigh + THIGH_ZERO)
|
||||
* θ_shank_link = -(motor_shank + motor_thigh) + SHANK_COUPLING_C
|
||||
* θ_foot_eff = θ_shank_link - FOOT_OFFSET
|
||||
*
|
||||
* IK → 电机角 (无符号):
|
||||
* motor_thigh = -(θ_hip + THIGH_ZERO)
|
||||
* motor_shank = -motor_thigh - θ_shank_link + SHANK_COUPLING_C
|
||||
* ============================================================ */
|
||||
|
||||
/* 硬件标定常量 */
|
||||
#define THIGH_ZERO_RAD 0.95f /* 大腿垂直时电机绝对角 (rad) */
|
||||
#define SHANK_VERTICAL_RAD 2.76f /* 小腿链节垂直时电机角 (rad) */
|
||||
#define FOOT_OFFSET_RAD DEG2RAD(40.0f) /* 足端安装偏置 40° */
|
||||
|
||||
/* 同步带耦合常量: C = 1.76 */
|
||||
#define SHANK_COUPLING_C 1.76f
|
||||
|
||||
/* 外八补偿角度 */
|
||||
#define LEG_SPLAY_ANGLE_RAD DEG2RAD(15.0f)
|
||||
|
||||
typedef struct {
|
||||
float L1; /* 大腿长 (mm) */
|
||||
float L2; /* 小腿到足端球心 (mm) */
|
||||
float foot_offset_rad; /* 足端偏置角 (rad) */
|
||||
float thigh_zero; /* 大腿垂直时电机角 */
|
||||
float shank_coupling_c; /* 耦合常量 */
|
||||
float splay_angle_rad; /* 外八倾斜角 */
|
||||
} RobotGeometry;
|
||||
|
||||
/* ============================================================
|
||||
* PD 增益 + 力矩前馈
|
||||
* ============================================================ */
|
||||
typedef struct {
|
||||
float kp_thigh;
|
||||
float kp_shank;
|
||||
float kd;
|
||||
float torque;
|
||||
} ControlGains;
|
||||
|
||||
/* ============================================================
|
||||
* VMC 全状态参数
|
||||
* ============================================================ */
|
||||
typedef struct {
|
||||
uint8_t enable;
|
||||
float kx;
|
||||
float kz;
|
||||
float torque_limit;
|
||||
} VmcConfig;
|
||||
|
||||
typedef struct {
|
||||
uint8_t enable;
|
||||
float kx;
|
||||
float kz;
|
||||
float bx;
|
||||
float bz;
|
||||
float gravity_ff;
|
||||
float torque_limit;
|
||||
} VmcFullConfig;
|
||||
|
||||
/* ============================================================
|
||||
* 步态轨迹参数 (运行时可调)
|
||||
* ============================================================ */
|
||||
typedef struct {
|
||||
float step_length;
|
||||
float step_height;
|
||||
float period;
|
||||
float start_z;
|
||||
float duty_cycle;
|
||||
float turn_rate;
|
||||
} GaitParams;
|
||||
|
||||
/* ============================================================
|
||||
* 完整步态配置集
|
||||
* 注: PostureConfig 和 GaitScaling 已废除
|
||||
* IK 直接输出电机角, 无需间接映射
|
||||
* ============================================================ */
|
||||
typedef struct {
|
||||
ControlGains swing_gains;
|
||||
ControlGains stance_gains;
|
||||
VmcConfig vmc;
|
||||
VmcFullConfig vmc_stance;
|
||||
VmcFullConfig vmc_swing;
|
||||
uint8_t use_full_vmc;
|
||||
} GaitProfile;
|
||||
|
||||
/* ============================================================
|
||||
* 预定义配置
|
||||
* ============================================================ */
|
||||
extern const RobotGeometry DEFAULT_GEOMETRY;
|
||||
extern const GaitProfile PROFILE_TROT_FORWARD;
|
||||
extern const GaitProfile PROFILE_TROT_WALK;
|
||||
extern const GaitProfile PROFILE_TROT_INPLACE;
|
||||
extern const ControlGains CRAWL_SWING_GAINS;
|
||||
extern const ControlGains CRAWL_STANCE_GAINS;
|
||||
|
||||
/* 腿部站立初始的 X 轴偏置，可用于改善因为重心靠后导致后腿受力过大的问题 */
|
||||
extern const float LEG_STANCE_X_OFFSET[LEG_COUNT];
|
||||
|
||||
/* 高度常量 (髋关节到足端的距离 mm) */
|
||||
#define PRONE_Z 150.0f
|
||||
#define STAND_Z 380.0f
|
||||
|
||||
#define ROBOT_MASS_KG 5.0f
|
||||
#define GRAVITY_FF_PER_LEG (ROBOT_MASS_KG * 9.81f / 4.0f)
|
||||
|
||||
#endif /* __GAIT_CONFIG_H__ */
|
||||
@@ -0,0 +1,251 @@
|
||||
/**
|
||||
* @file leg.c
|
||||
* @brief 腿部运动学 — 基于硬件实测标定参数
|
||||
*
|
||||
* 物理模型:
|
||||
* 二连杆串联 + 同步带1:1 + 40度足端偏置
|
||||
*
|
||||
* 正运动学 (电机角 → 链节角 → 足端):
|
||||
* θ_hip = -(motor_thigh + thigh_zero)
|
||||
* θ_shank_link = -(motor_shank + motor_thigh) + shank_coupling_c
|
||||
* φ = θ_shank_link - foot_offset (等效足端方向)
|
||||
* x = L1*sin(θ_hip) + L2*sin(φ)
|
||||
* z = L1*cos(θ_hip) + L2*cos(φ)
|
||||
*
|
||||
* 逆运动学 (足端 → 电机角):
|
||||
* 标准二连杆IK求 θ_hip 和 φ
|
||||
* θ_shank_link = φ + foot_offset
|
||||
* motor_thigh = -(θ_hip + thigh_zero)
|
||||
* motor_shank = -motor_thigh - θ_shank_link + shank_coupling_c
|
||||
*
|
||||
* IK 选择"后倾解" (θ_hip = ψ - β):
|
||||
* 大腿略向后倾, 膝关节在前方弯曲
|
||||
* 这是该四足的自然站立构型
|
||||
*/
|
||||
|
||||
#include "leg.h"
|
||||
#include <math.h>
|
||||
|
||||
/* ============================================================
|
||||
* 逆运动学 — 直接输出无符号电机角
|
||||
*
|
||||
* X: 足端前向偏移 (mm), 前向为正
|
||||
* Z: 足端向下距离 (mm), 向下为正
|
||||
*
|
||||
* 使用"后倾解": θ_hip = ψ - β
|
||||
* 验证: 趴下(motors≈0), 垂直(thigh=-1.0, shank=2.76)
|
||||
* ============================================================ */
|
||||
void Inverse_Calculation(float X, float Z,
|
||||
float *motor_thigh, float *motor_shank,
|
||||
const RobotGeometry *geom)
|
||||
{
|
||||
float L1 = geom->L1;
|
||||
float L2 = geom->L2;
|
||||
float foff = geom->foot_offset_rad;
|
||||
float tzero = geom->thigh_zero;
|
||||
float sc = geom->shank_coupling_c;
|
||||
float splay = geom->splay_angle_rad;
|
||||
|
||||
/* 补偿15度外八倾角: 实际腿在倾斜平面内, 达到垂直高度Z需要的计算长度会变长 */
|
||||
float Z_leg = Z / cosf(splay);
|
||||
|
||||
float L = sqrtf(X * X + Z_leg * Z_leg);
|
||||
|
||||
/* 防止超出工作空间 */
|
||||
float L_max = L1 + L2 - 1.0f;
|
||||
float L_min = fabsf(L1 - L2) + 1.0f;
|
||||
if (L > L_max) L = L_max;
|
||||
if (L < L_min) L = L_min;
|
||||
|
||||
/* --- 标准二连杆 IK --- */
|
||||
|
||||
/* 膝关节内角 (L1和L2之间的三角形内角) */
|
||||
float cos_knee_int = (L1 * L1 + L2 * L2 - L * L) / (2.0f * L1 * L2);
|
||||
if (cos_knee_int > 1.0f) cos_knee_int = 1.0f;
|
||||
if (cos_knee_int < -1.0f) cos_knee_int = -1.0f;
|
||||
|
||||
/* 髋关节处三角形内角 */
|
||||
float cos_hip_int = (L * L + L1 * L1 - L2 * L2) / (2.0f * L * L1);
|
||||
if (cos_hip_int > 1.0f) cos_hip_int = 1.0f;
|
||||
if (cos_hip_int < -1.0f) cos_hip_int = -1.0f;
|
||||
float beta = acosf(cos_hip_int);
|
||||
|
||||
/* 髋到足连线与竖直方向的夹角 */
|
||||
float psi = atan2f(X, Z_leg);
|
||||
|
||||
/* "后倾解": θ_hip = ψ - β
|
||||
* 大腿向后倾斜, 膝关节在前方 — 匹配该四足的自然构型 */
|
||||
float theta_hip = psi - beta;
|
||||
|
||||
/* 等效足端方向角 φ (第二连杆在IK空间中的绝对角) */
|
||||
float sin_phi = (X - L1 * sinf(theta_hip)) / L2;
|
||||
float cos_phi = (Z_leg - L1 * cosf(theta_hip)) / L2;
|
||||
float phi = atan2f(sin_phi, cos_phi);
|
||||
|
||||
/* 小腿链节角 = φ + 足端偏置 */
|
||||
float theta_shank_link = phi + foff;
|
||||
|
||||
/* --- 转换为电机角 ---
|
||||
* 恢复物理耦合: 之前因为趴下高度的问题误以为解耦, 但实际上用户的 PRONE_Z 是一个较小的非完全趴下的高度
|
||||
* 电机实际上是带有跟随耦合的机制, 小腿绝对角度受大腿牵连!
|
||||
* θ_shank_link = -(ms + mt) + sc
|
||||
*/
|
||||
float mt = -(theta_hip + tzero);
|
||||
float ms = -mt - theta_shank_link + sc;
|
||||
|
||||
*motor_thigh = mt;
|
||||
*motor_shank = ms;
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 摆线轨迹生成
|
||||
* ============================================================ */
|
||||
void Gen_Cycloid_Trajectory(float phase, float swing_ratio,
|
||||
float Xs, float Xe, float Zs, float h,
|
||||
float *x, float *z)
|
||||
{
|
||||
phase = fmodf(phase, 1.0f);
|
||||
if (phase < 0.0f) phase += 1.0f;
|
||||
|
||||
if (phase < swing_ratio)
|
||||
{
|
||||
float t = phase / swing_ratio;
|
||||
/* 标准摆线: 起步和落地时速度和加速度均为0，减小冲击 */
|
||||
*x = Xs + (Xe - Xs) * (t - 1.0f / (2.0f * PI) * sinf(2.0f * PI * t));
|
||||
*z = Zs - h * (1.0f - cosf(2.0f * PI * t)) / 2.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 支撑相，地面上匀速滑行/静止 */
|
||||
float t = (phase - swing_ratio) / (1.0f - swing_ratio);
|
||||
*x = Xe + (Xs - Xe) * t;
|
||||
*z = Zs;
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 三次贝塞尔曲线轨迹生成
|
||||
* 比摆线具有更高的提腿速度，灵活性更强
|
||||
* ============================================================ */
|
||||
void Gen_Bezier_Trajectory(float phase, float swing_ratio,
|
||||
float Xs, float Xe, float Zs, float h,
|
||||
float *x, float *z)
|
||||
{
|
||||
phase = fmodf(phase, 1.0f);
|
||||
if (phase < 0.0f) phase += 1.0f;
|
||||
|
||||
if (phase < swing_ratio)
|
||||
{
|
||||
float tau = phase / swing_ratio;
|
||||
float tau2 = tau * tau;
|
||||
float tau3 = tau2 * tau;
|
||||
float one_minus_tau = 1.0f - tau;
|
||||
float one_minus_tau2 = one_minus_tau * one_minus_tau;
|
||||
float one_minus_tau3 = one_minus_tau2 * one_minus_tau;
|
||||
|
||||
float step_length = Xe - Xs;
|
||||
|
||||
/* 贝塞尔多项式系数 */
|
||||
float b = 3.0f * one_minus_tau2 * tau;
|
||||
float c = 3.0f * one_minus_tau * tau2;
|
||||
float d = tau3;
|
||||
|
||||
/* X方向控制点: 匀速推进 P1=1/3, P2=2/3
|
||||
* 若要前扫/后扫不对称，可调整这两个除数参数 */
|
||||
*x = Xs + b * (step_length / 3.0f) +
|
||||
c * (2.0f * step_length / 3.0f) +
|
||||
d * step_length;
|
||||
|
||||
/* Z方向控制点:
|
||||
* 如果设为h/2，最高点只有 0.375*h。
|
||||
* 为了让它刚好在 tau=0.5 时抬高 h，控制点需设为 4/3 * h */
|
||||
float p_z = h * 4.0f / 3.0f;
|
||||
*z = Zs - (b * p_z + c * p_z);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 支撑相，地面上匀速滑行/静止 */
|
||||
float t = (phase - swing_ratio) / (1.0f - swing_ratio);
|
||||
*x = Xe + (Xs - Xe) * t;
|
||||
*z = Zs;
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 正运动学 — 电机无符号角 → 足端位置 (mm)
|
||||
*
|
||||
* 从电机角还原链节角, 再算足端位置
|
||||
* ============================================================ */
|
||||
void FK_LegPosition(float mt, float ms,
|
||||
const RobotGeometry *geom,
|
||||
float *x_out, float *z_out)
|
||||
{
|
||||
float L1 = geom->L1;
|
||||
float L2 = geom->L2;
|
||||
float foff = geom->foot_offset_rad;
|
||||
float tzero = geom->thigh_zero;
|
||||
float sc = geom->shank_coupling_c;
|
||||
float splay = geom->splay_angle_rad;
|
||||
|
||||
/* 电机角 → 链节角 */
|
||||
float theta_hip = -(mt + tzero);
|
||||
float theta_shank_link = -(ms + mt) + sc;
|
||||
|
||||
/* 等效足端方向 */
|
||||
float phi = theta_shank_link - foff;
|
||||
|
||||
*x_out = L1 * sinf(theta_hip) + L2 * sinf(phi);
|
||||
|
||||
/* 腿平面内的Z长度，再投射回垂直身体距离 */
|
||||
float Z_leg = L1 * cosf(theta_hip) + L2 * cosf(phi);
|
||||
*z_out = Z_leg * cosf(splay);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* 足端速度 — Jacobian × 电机角速度
|
||||
*
|
||||
* 设 q = [mt, ms]^T (无符号电机角)
|
||||
* θ_hip = -(mt + C1) → dθ_hip/dmt = -1, dθ_hip/dms = 0
|
||||
* θ_sl = -(ms + mt) + C2 → dθ_sl/dmt = -1, dθ_sl/dms = -1
|
||||
* φ = θ_sl - foff → dφ/dmt = -1, dφ/dms = -1
|
||||
*
|
||||
* x = L1*sin(θh) + L2*sin(φ)
|
||||
* dx/dmt = L1*cos(θh)*(-1) + L2*cos(φ)*(-1) = -(L1*cos(θh) + L2*cos(φ))
|
||||
* dx/dms = L2*cos(φ)*(-1) = -L2*cos(φ)
|
||||
*
|
||||
* z = L1*cos(θh) + L2*cos(φ)
|
||||
* dz/dmt = -L1*sin(θh)*(-1) - L2*sin(φ)*(-1) = L1*sin(θh) + L2*sin(φ)
|
||||
* dz/dms = -L2*sin(φ)*(-1) = L2*sin(φ)
|
||||
* ============================================================ */
|
||||
void FK_LegVelocity(float mt, float ms,
|
||||
float vel_mt, float vel_ms,
|
||||
const RobotGeometry *geom,
|
||||
float *xdot, float *zdot)
|
||||
{
|
||||
float L1 = geom->L1;
|
||||
float L2 = geom->L2;
|
||||
float foff = geom->foot_offset_rad;
|
||||
float tzero = geom->thigh_zero;
|
||||
float sc = geom->shank_coupling_c;
|
||||
float splay = geom->splay_angle_rad;
|
||||
|
||||
float theta_hip = -(mt + tzero);
|
||||
float theta_sl = -(ms + mt) + sc;
|
||||
float phi = theta_sl - foff;
|
||||
|
||||
float ch = cosf(theta_hip);
|
||||
float sh = sinf(theta_hip);
|
||||
float cp = cosf(phi);
|
||||
float sp = sinf(phi);
|
||||
|
||||
/* Jacobian J = [dx/dmt, dx/dms; dz/dmt, dz/dms] */
|
||||
float Jx_mt = -(L1 * ch + L2 * cp);
|
||||
float Jx_ms = -(L2 * cp);
|
||||
|
||||
float cos_splay = cosf(splay);
|
||||
float Jz_mt = (L1 * sh + L2 * sp) * cos_splay;
|
||||
float Jz_ms = (L2 * sp) * cos_splay;
|
||||
|
||||
*xdot = Jx_mt * vel_mt + Jx_ms * vel_ms;
|
||||
*zdot = Jz_mt * vel_mt + Jz_ms * vel_ms;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
#ifndef __LEG_H__
|
||||
#define __LEG_H__
|
||||
|
||||
#include <math.h>
|
||||
#include "gait_config.h"
|
||||
|
||||
/* ============================================================
|
||||
* 逆运动学 — 直接输出无符号电机角
|
||||
*
|
||||
* 输入: X (前后偏移 mm, 前向正), Z (高度 mm, 向下正)
|
||||
* 输出: motor_thigh (大腿无符号电机角 rad)
|
||||
* motor_shank (小腿无符号电机角 rad)
|
||||
*
|
||||
* 调用方在外部乘 SIDE_SIGNS[leg] 做左右镜像
|
||||
*
|
||||
* 内部使用:
|
||||
* θ_hip = ψ - β (后倾解, 适合膝前弯四足)
|
||||
* motor_thigh = -(θ_hip + thigh_zero)
|
||||
* motor_shank = -motor_thigh - θ_shank + shank_coupling_c
|
||||
* ============================================================ */
|
||||
void Inverse_Calculation(float X, float Z,
|
||||
float *motor_thigh, float *motor_shank,
|
||||
const RobotGeometry *geom);
|
||||
|
||||
/* ============================================================
|
||||
* 摆线轨迹生成
|
||||
* ============================================================ */
|
||||
void Gen_Cycloid_Trajectory(float phase, float swing_ratio,
|
||||
float Xs, float Xe, float Zs, float h,
|
||||
float *x, float *z);
|
||||
|
||||
/* ============================================================
|
||||
* 三次贝塞尔曲线轨迹生成
|
||||
* ============================================================ */
|
||||
void Gen_Bezier_Trajectory(float phase, float swing_ratio,
|
||||
float Xs, float Xe, float Zs, float h,
|
||||
float *x, float *z);
|
||||
|
||||
/* ============================================================
|
||||
* 正运动学 — 电机无符号角 → 足端位置 (mm)
|
||||
* 注意: 输入是无符号电机角 (已去掉SIDE_SIGNS)
|
||||
* ============================================================ */
|
||||
void FK_LegPosition(float motor_thigh_unsigned, float motor_shank_unsigned,
|
||||
const RobotGeometry *geom,
|
||||
float *x_out, float *z_out);
|
||||
|
||||
/* ============================================================
|
||||
* 足端速度 — Jacobian × 电机角速度
|
||||
* 输入: 无符号电机角 + 无符号角速度
|
||||
* ============================================================ */
|
||||
void FK_LegVelocity(float motor_thigh_unsigned, float motor_shank_unsigned,
|
||||
float vel_thigh_unsigned, float vel_shank_unsigned,
|
||||
const RobotGeometry *geom,
|
||||
float *xdot, float *zdot);
|
||||
|
||||
#endif /* __LEG_H__ */
|
||||
@@ -0,0 +1,598 @@
|
||||
#include "lingzu_motor.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "can.h"
|
||||
#include <stdio.h> // For potential debug
|
||||
#include <string.h> // For memset if needed
|
||||
#include <math.h>
|
||||
|
||||
#ifndef isnan
|
||||
#define isnan(x) ((x) != (x))
|
||||
#endif
|
||||
|
||||
#define PI 3.1415926535f
|
||||
#define RS02_SAFE_POS_RAD (200.0f * PI / 180.0f)
|
||||
|
||||
Can_Bus_Data_Struct CAN_1;
|
||||
Can_Bus_Data_Struct CAN_2;
|
||||
Motor_CAN_Recieve_Struct Motor_Recieve_Single_CAN1;
|
||||
Motor_CAN_Recieve_Struct Motor_Recieve_Single_CAN2;
|
||||
|
||||
// Private variables for sending
|
||||
static CanTxMsgTypeDef txMsg_CAN;
|
||||
|
||||
static Motor_CAN_Send_Struct* rs02_get_send_struct(uint8_t motor_index)
|
||||
{
|
||||
switch (motor_index)
|
||||
{
|
||||
case RS02_MOTOR_1: return &CAN_1.ID_1_Motor_send;
|
||||
case RS02_MOTOR_2: return &CAN_1.ID_2_Motor_send;
|
||||
case RS02_MOTOR_3: return &CAN_1.ID_3_Motor_send;
|
||||
case RS02_MOTOR_4: return &CAN_1.ID_4_Motor_send;
|
||||
case RS02_MOTOR_5: return &CAN_2.ID_1_Motor_send;
|
||||
case RS02_MOTOR_6: return &CAN_2.ID_2_Motor_send;
|
||||
case RS02_MOTOR_7: return &CAN_2.ID_3_Motor_send;
|
||||
case RS02_MOTOR_8: return &CAN_2.ID_4_Motor_send;
|
||||
default: return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
static Motor_CAN_Recieve_Struct* rs02_get_recv_struct(uint8_t motor_index)
|
||||
{
|
||||
switch (motor_index)
|
||||
{
|
||||
case RS02_MOTOR_1: return &CAN_1.ID_1_Motor_recieve;
|
||||
case RS02_MOTOR_2: return &CAN_1.ID_2_Motor_recieve;
|
||||
case RS02_MOTOR_3: return &CAN_1.ID_3_Motor_recieve;
|
||||
case RS02_MOTOR_4: return &CAN_1.ID_4_Motor_recieve;
|
||||
case RS02_MOTOR_5: return &CAN_2.ID_1_Motor_recieve;
|
||||
case RS02_MOTOR_6: return &CAN_2.ID_2_Motor_recieve;
|
||||
case RS02_MOTOR_7: return &CAN_2.ID_3_Motor_recieve;
|
||||
case RS02_MOTOR_8: return &CAN_2.ID_4_Motor_recieve;
|
||||
default: return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void rs02_set_target_rad(uint8_t motor_index, float position, float speed, float kp, float kd, float torque)
|
||||
{
|
||||
Motor_CAN_Send_Struct *send_struct = rs02_get_send_struct(motor_index);
|
||||
if (send_struct == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// motor_index 取值范围为 1~8，对应 1~8 号关节电机
|
||||
if (motor_index == 0 || motor_index > 8)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// 对目标位置做绝对角度限幅，防止指令过大导致潜在疯转
|
||||
float p = position;
|
||||
if (p > RS02_SAFE_POS_RAD)
|
||||
{
|
||||
p = RS02_SAFE_POS_RAD;
|
||||
}
|
||||
else if (p < -RS02_SAFE_POS_RAD)
|
||||
{
|
||||
p = -RS02_SAFE_POS_RAD;
|
||||
}
|
||||
|
||||
send_struct->position = p;
|
||||
send_struct->speed = speed;
|
||||
send_struct->kp = kp;
|
||||
send_struct->kd = kd;
|
||||
send_struct->torque = torque;
|
||||
}
|
||||
|
||||
float rs02_get_position_rad(uint8_t motor_index)
|
||||
{
|
||||
Motor_CAN_Recieve_Struct *recv_struct = rs02_get_recv_struct(motor_index);
|
||||
if (recv_struct == NULL)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
return recv_struct->current_position_f;
|
||||
}
|
||||
|
||||
float rs02_get_velocity_rad(uint8_t motor_index)
|
||||
{
|
||||
Motor_CAN_Recieve_Struct *recv_struct = rs02_get_recv_struct(motor_index);
|
||||
if (recv_struct == NULL)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
return recv_struct->current_speed_f;
|
||||
}
|
||||
|
||||
float rs02_get_error_rad(uint8_t motor_index, float target_position)
|
||||
{
|
||||
float current = rs02_get_position_rad(motor_index);
|
||||
return target_position - current;
|
||||
}
|
||||
|
||||
/* Per-motor wrapper functions removed in refactoring.
|
||||
* Use rs02_set_target_rad(motor_index, ...) directly. */
|
||||
|
||||
int float_to_uint(float x, float x_min, float x_max, int bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
if(x > x_max) x = x_max;
|
||||
else if(x < x_min) x = x_min;
|
||||
return (int) ((x-offset)*((float)((1<<bits)-1))/span);
|
||||
}
|
||||
|
||||
// 适配 Python 驱动的映射算法: int(((x / limit) + 1.0) * 32767.0)
|
||||
// x_max 对应 limit. x_min 对应 -limit.
|
||||
int float_to_uint_mit(float x, float limit, int bits)
|
||||
{
|
||||
float max = limit;
|
||||
float min = -limit;
|
||||
if(x > max) x = max;
|
||||
else if(x < min) x = min;
|
||||
|
||||
// Python Logic: ((x / limit) + 1.0) * 32767.0
|
||||
// Maps [-limit, limit] to [0, 65534] approx
|
||||
// Using 32767.0 as scale factor
|
||||
return (int)(((x / limit) + 1.0f) * 32767.0f);
|
||||
}
|
||||
|
||||
// Kp/Kd 映射: int((val / limit) * 65535.0)
|
||||
// Maps [0, limit] to [0, 65535]
|
||||
int float_to_uint_mit_param(float x, float limit)
|
||||
{
|
||||
if (x > limit) x = limit;
|
||||
if (x < 0) x = 0;
|
||||
return (int)((x / limit) * 65535.0f);
|
||||
}
|
||||
|
||||
float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
|
||||
}
|
||||
|
||||
// 适配 Python 驱动的反向映射 (仅用于参考，接收时可能用不上，接收用 uint_to_float 即可)
|
||||
// 实际上 Python 接收解析是 struct.unpack，得到的是 uint16。
|
||||
// 并没有展示 uint -> float 的逻辑，但通常是对称的。
|
||||
// 假设接收也是同样的逻辑，或者直接用 float_to_uint 的逆运算。
|
||||
// 但根据 constants.py, Python 并没有做 float conversion regarding feedback except printing?
|
||||
// robstride_driver.py line 295: unpacks to u16.
|
||||
// line 298: p_limit etc.
|
||||
// The code cuts off at line 301. It presumably converts u16 back to float.
|
||||
// Let's assume symmetric mapping.
|
||||
float uint_to_float_mit(int x_int, float limit, int bits)
|
||||
{
|
||||
// y = ((x/L) + 1) * 32767
|
||||
// y/32767 = x/L + 1
|
||||
// x/L = y/32767 - 1
|
||||
// x = (y/32767 - 1) * L
|
||||
return ((float)x_int / 32767.0f - 1.0f) * limit;
|
||||
}
|
||||
|
||||
// Kp/Kd/Torque(Received?)
|
||||
// Feedback T is signed int16? Python unpacks >HHHH, but names it t_i16?
|
||||
// If unpacks as H, it is u16.
|
||||
// If the feedback torque is indeed signed int16 in the motor firmware, struct.unpack should use 'h'.
|
||||
// If Python uses 'H', it treats it as unsigned.
|
||||
// Let's assume standard mapping for now.
|
||||
|
||||
// 通讯类型 3: 电机使能运行
|
||||
void Motor_Enable(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data)
|
||||
{
|
||||
hcan->pTxMsg = &txMsg_CAN;
|
||||
|
||||
hcan->pTxMsg->StdId = 0;
|
||||
// bit28~24: 0x3 (通讯类型)
|
||||
// bit23~8: bit15~8 用标识主CAN_ID (0xFD) -> Extra Data
|
||||
// bit7~0: 目标电机CAN_ID
|
||||
|
||||
// Python: _send_command(ENABLE, host_id, motor.id)
|
||||
// Extra Data = host_id
|
||||
// Device ID = motor.id
|
||||
|
||||
uint32_t cmd_type = 0x3;
|
||||
uint32_t host_id = 0xFD; // Master ID
|
||||
uint32_t motor_id = Motor_Data->id;
|
||||
|
||||
hcan->pTxMsg->ExtId = (cmd_type << 24) | (host_id << 8) | motor_id;
|
||||
hcan->pTxMsg->IDE = CAN_ID_EXT;
|
||||
hcan->pTxMsg->RTR = CAN_RTR_DATA;
|
||||
hcan->pTxMsg->DLC = 8;
|
||||
|
||||
for(int i=0; i<8; i++)
|
||||
{
|
||||
hcan->pTxMsg->Data[i] = 0;
|
||||
}
|
||||
|
||||
// 2025-01-26 Mod: Set zero_sta = 1 (Data[0])
|
||||
// 0: 0-2PI (Default, may take long path)
|
||||
// 1: -PI~PI (Shortest path/优弧)
|
||||
hcan->pTxMsg->Data[0] = 1;
|
||||
|
||||
// Retry mechanism for reliable transmission
|
||||
uint8_t retry = 0;
|
||||
while(HAL_CAN_Transmit(hcan, 2) != HAL_OK && retry < 5)
|
||||
{
|
||||
retry++;
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
// 通讯类型 4: 电机停止运行
|
||||
void Motor_Disable(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data)
|
||||
{
|
||||
hcan->pTxMsg = &txMsg_CAN;
|
||||
|
||||
hcan->pTxMsg->StdId = 0;
|
||||
// bit28~24: 0x4 (通讯类型)
|
||||
// bit23~8: Extra Data (Host ID)
|
||||
// bit7~0: 目标电机CAN_ID
|
||||
uint32_t cmd_type = 0x4;
|
||||
uint32_t host_id = 0xFD;
|
||||
uint32_t motor_id = Motor_Data->id;
|
||||
|
||||
hcan->pTxMsg->ExtId = (cmd_type << 24) | (host_id << 8) | motor_id;
|
||||
hcan->pTxMsg->IDE = CAN_ID_EXT;
|
||||
hcan->pTxMsg->RTR = CAN_RTR_DATA;
|
||||
hcan->pTxMsg->DLC = 8;
|
||||
|
||||
for(int i=0; i<8; i++)
|
||||
{
|
||||
hcan->pTxMsg->Data[i] = 0;
|
||||
}
|
||||
// 正常停止时Data清0
|
||||
|
||||
uint8_t retry = 0;
|
||||
while(HAL_CAN_Transmit(hcan, 2) != HAL_OK && retry < 5)
|
||||
{
|
||||
retry++;
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
// 通讯类型 6: 设置电机机械零位
|
||||
// 注意: 此指令仅将当前位置重置为0，不会修改 zero_sta 标志位。
|
||||
// 若需修改位置范围(0~2PI 或 -PI~PI)，请使用上位机修改 zero_sta 并保存。
|
||||
void Motor_Zore(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data)
|
||||
{
|
||||
hcan->pTxMsg = &txMsg_CAN;
|
||||
|
||||
hcan->pTxMsg->StdId = 0;
|
||||
// bit28~24: 0x6 (通讯类型)
|
||||
// bit23~8: Extra Data (Host ID)
|
||||
// bit7~0: 目标电机CAN_ID
|
||||
uint32_t cmd_type = 0x6;
|
||||
uint32_t host_id = 0xFD;
|
||||
uint32_t motor_id = Motor_Data->id;
|
||||
|
||||
hcan->pTxMsg->ExtId = (cmd_type << 24) | (host_id << 8) | motor_id;
|
||||
hcan->pTxMsg->IDE = CAN_ID_EXT;
|
||||
hcan->pTxMsg->RTR = CAN_RTR_DATA;
|
||||
hcan->pTxMsg->DLC = 8;
|
||||
|
||||
for(int i=0; i<8; i++)
|
||||
{
|
||||
hcan->pTxMsg->Data[i] = 0;
|
||||
}
|
||||
hcan->pTxMsg->Data[0] = 1; // 触发设置零点动作 (并非设置 zero_sta=1)
|
||||
|
||||
uint8_t retry = 0;
|
||||
while(HAL_CAN_Transmit(hcan, 2) != HAL_OK && retry < 5)
|
||||
{
|
||||
retry++;
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
// 通讯类型 1: 遥控模式电机控制指令
|
||||
void CAN_Send_Control(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data)
|
||||
{
|
||||
// Safety check: Do not send if position is NaN
|
||||
if (isnan(Motor_Data->position))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
CanTxMsgTypeDef txMsg_Control;
|
||||
hcan->pTxMsg = &txMsg_Control;
|
||||
|
||||
txMsg_Control.StdId = 0;
|
||||
txMsg_Control.IDE = CAN_ID_EXT;
|
||||
txMsg_Control.RTR = CAN_RTR_DATA;
|
||||
txMsg_Control.DLC = 8;
|
||||
|
||||
// bit28~24: 0x1 (指令)
|
||||
// bit23~8: Extra Data (Torque)
|
||||
// Bit7~0: 目标电机CAN_ID
|
||||
|
||||
uint32_t cmd_type = 0x1;
|
||||
|
||||
// ExtId力矩 (Mapped T_MIN ~ T_MAX -> 0~65535)
|
||||
// Python Logic: t_u16 = int(((torque / t_limit) + 1.0) * 32767.0)
|
||||
uint16_t torque_ext_id = float_to_uint_mit(Motor_Data->torque, T_MAX, 16); // T_MAX corresponds to limit
|
||||
|
||||
txMsg_Control.ExtId = (cmd_type << 24) | (torque_ext_id << 8) | Motor_Data->id;
|
||||
|
||||
// Data Structure: Big Endian [Pos, Vel, Kp, Kd]
|
||||
|
||||
// Byte0~1: 角度 (Mapped)
|
||||
// Python Logic: p_u16 = int(((position / p_limit) + 1.0) * 32767.0)
|
||||
uint16_t pos_int = float_to_uint_mit(Motor_Data->position, P_MAX, 16);
|
||||
txMsg_Control.Data[0] = pos_int >> 8; // 高字节在前 (Big Endian)
|
||||
txMsg_Control.Data[1] = pos_int & 0xFF; // 低字节在后
|
||||
|
||||
// Byte2~3: 速度 (Mapped)
|
||||
// Python Logic: v_u16 = int(((velocity / v_limit) + 1.0) * 32767.0)
|
||||
uint16_t spd_int = float_to_uint_mit(Motor_Data->speed, V_MAX, 16);
|
||||
txMsg_Control.Data[2] = spd_int >> 8;
|
||||
txMsg_Control.Data[3] = spd_int & 0xFF;
|
||||
|
||||
// Byte4~5: Kp (Mapped 0~Limit -> 0~65535)
|
||||
uint16_t kp_int = float_to_uint_mit_param(Motor_Data->kp, KP_MAX);
|
||||
txMsg_Control.Data[4] = kp_int >> 8;
|
||||
txMsg_Control.Data[5] = kp_int & 0xFF;
|
||||
|
||||
// Byte6~7: Kd (Mapped 0~Limit -> 0~65535)
|
||||
uint16_t kd_int = float_to_uint_mit_param(Motor_Data->kd, KD_MAX);
|
||||
txMsg_Control.Data[6] = kd_int >> 8;
|
||||
txMsg_Control.Data[7] = kd_int & 0xFF;
|
||||
|
||||
// 2025-01-26 Fix: Add retry logic for CAN Mailbox Full
|
||||
// If HAL_CAN_Transmit returns HAL_BUSY, we should retry.
|
||||
uint8_t retry = 0;
|
||||
HAL_StatusTypeDef status;
|
||||
|
||||
// Explicitly check if mailbox is full (for old HAL lib, Transmit is blocking if timeout != 0)
|
||||
// But if timeout occurs, it returns HAL_TIMEOUT.
|
||||
// If all mailboxes are busy, it waits until timeout.
|
||||
// We should try a few times.
|
||||
|
||||
do {
|
||||
status = HAL_CAN_Transmit(hcan, 2); // 2ms timeout per attempt
|
||||
if (status == HAL_OK) break;
|
||||
|
||||
retry++;
|
||||
// If we are stuck in a busy loop, maybe we should yield or delay slightly more?
|
||||
// But we want to send ASAP.
|
||||
} while (retry < 5);
|
||||
}
|
||||
|
||||
void Lingzu_Motor_Init_Structs(void)
|
||||
{
|
||||
// Initialize CAN 1 Motor Structs
|
||||
CAN_1.ID_1_Motor_send.id=1;
|
||||
CAN_1.ID_1_Motor_send.res=0x04;
|
||||
CAN_1.ID_1_Motor_send.max_position=2;
|
||||
CAN_1.ID_1_Motor_send.min_position=-2;
|
||||
|
||||
CAN_1.ID_2_Motor_send.id=2;
|
||||
CAN_1.ID_2_Motor_send.res=0x04;
|
||||
CAN_1.ID_2_Motor_send.max_position=2;
|
||||
CAN_1.ID_2_Motor_send.min_position=-2;
|
||||
|
||||
CAN_1.ID_3_Motor_send.id=3;
|
||||
CAN_1.ID_3_Motor_send.res=0x04;
|
||||
CAN_1.ID_3_Motor_send.max_position=2;
|
||||
CAN_1.ID_3_Motor_send.min_position=-2;
|
||||
|
||||
CAN_1.ID_4_Motor_send.id=4;
|
||||
CAN_1.ID_4_Motor_send.res=0x04;
|
||||
|
||||
// Initialize CAN 2 Motor Structs
|
||||
CAN_2.ID_1_Motor_send.id=1;
|
||||
CAN_2.ID_1_Motor_send.res=0x04;
|
||||
CAN_2.ID_1_Motor_send.max_position=2;
|
||||
CAN_2.ID_1_Motor_send.min_position=-2;
|
||||
|
||||
CAN_2.ID_2_Motor_send.id=2;
|
||||
CAN_2.ID_2_Motor_send.res=0x04;
|
||||
CAN_2.ID_2_Motor_send.max_position=2;
|
||||
CAN_2.ID_2_Motor_send.min_position=-2;
|
||||
|
||||
CAN_2.ID_3_Motor_send.id=3;
|
||||
CAN_2.ID_3_Motor_send.res=0x04;
|
||||
CAN_2.ID_3_Motor_send.max_position=2;
|
||||
CAN_2.ID_3_Motor_send.min_position=-2;
|
||||
|
||||
CAN_2.ID_4_Motor_send.id=4;
|
||||
CAN_2.ID_4_Motor_send.res=0x04;
|
||||
CAN_2.ID_4_Motor_send.max_position=2;
|
||||
CAN_2.ID_4_Motor_send.min_position=-2;
|
||||
}
|
||||
|
||||
void ENABLE_ALL_LINGZU_MOTORS(void)
|
||||
{
|
||||
Motor_Enable(&hcan1, &CAN_1.ID_1_Motor_send);
|
||||
osDelay(2);
|
||||
Motor_Enable(&hcan2, &CAN_2.ID_1_Motor_send);
|
||||
osDelay(2);
|
||||
|
||||
Motor_Enable(&hcan1, &CAN_1.ID_2_Motor_send);
|
||||
osDelay(2);
|
||||
Motor_Enable(&hcan2, &CAN_2.ID_2_Motor_send);
|
||||
osDelay(2);
|
||||
|
||||
Motor_Enable(&hcan1, &CAN_1.ID_3_Motor_send);
|
||||
osDelay(2);
|
||||
Motor_Enable(&hcan2, &CAN_2.ID_3_Motor_send);
|
||||
osDelay(2);
|
||||
|
||||
Motor_Enable(&hcan1, &CAN_1.ID_4_Motor_send);
|
||||
osDelay(2);
|
||||
Motor_Enable(&hcan2, &CAN_2.ID_4_Motor_send);
|
||||
osDelay(2);
|
||||
}
|
||||
|
||||
void DISABLE_ALL_LINGZU_MOTORS(void)
|
||||
{
|
||||
Motor_Disable(&hcan1, &CAN_1.ID_1_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Disable(&hcan2, &CAN_2.ID_1_Motor_send);
|
||||
osDelay(1);
|
||||
|
||||
Motor_Disable(&hcan1, &CAN_1.ID_2_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Disable(&hcan2, &CAN_2.ID_2_Motor_send);
|
||||
osDelay(1);
|
||||
|
||||
Motor_Disable(&hcan1, &CAN_1.ID_3_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Disable(&hcan2, &CAN_2.ID_3_Motor_send);
|
||||
osDelay(1);
|
||||
|
||||
Motor_Disable(&hcan1, &CAN_1.ID_4_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Disable(&hcan2, &CAN_2.ID_4_Motor_send);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
void ZERO_ALL_LINGZU_MOTORS(void)
|
||||
{
|
||||
Motor_Zore(&hcan1, &CAN_1.ID_1_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Zore(&hcan2, &CAN_2.ID_1_Motor_send);
|
||||
osDelay(1);
|
||||
|
||||
Motor_Zore(&hcan1, &CAN_1.ID_2_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Zore(&hcan2, &CAN_2.ID_2_Motor_send);
|
||||
osDelay(1);
|
||||
|
||||
Motor_Zore(&hcan1, &CAN_1.ID_3_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Zore(&hcan2, &CAN_2.ID_3_Motor_send);
|
||||
osDelay(1);
|
||||
|
||||
Motor_Zore(&hcan1, &CAN_1.ID_4_Motor_send);
|
||||
osDelay(1);
|
||||
Motor_Zore(&hcan2, &CAN_2.ID_4_Motor_send);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
void LINGZU_All_Motors_Limp(void)
|
||||
{
|
||||
CAN_1.ID_1_Motor_send.torque = 0; CAN_1.ID_1_Motor_send.kp = 0; CAN_1.ID_1_Motor_send.kd = 0; CAN_1.ID_1_Motor_send.speed = 0;
|
||||
CAN_1.ID_2_Motor_send.torque = 0; CAN_1.ID_2_Motor_send.kp = 0; CAN_1.ID_2_Motor_send.kd = 0; CAN_1.ID_2_Motor_send.speed = 0;
|
||||
CAN_1.ID_3_Motor_send.torque = 0; CAN_1.ID_3_Motor_send.kp = 0; CAN_1.ID_3_Motor_send.kd = 0; CAN_1.ID_3_Motor_send.speed = 0;
|
||||
CAN_1.ID_4_Motor_send.torque = 0; CAN_1.ID_4_Motor_send.kp = 0; CAN_1.ID_4_Motor_send.kd = 0; CAN_1.ID_4_Motor_send.speed = 0;
|
||||
|
||||
CAN_2.ID_1_Motor_send.torque = 0; CAN_2.ID_1_Motor_send.kp = 0; CAN_2.ID_1_Motor_send.kd = 0; CAN_2.ID_1_Motor_send.speed = 0;
|
||||
CAN_2.ID_2_Motor_send.torque = 0; CAN_2.ID_2_Motor_send.kp = 0; CAN_2.ID_2_Motor_send.kd = 0; CAN_2.ID_2_Motor_send.speed = 0;
|
||||
CAN_2.ID_3_Motor_send.torque = 0; CAN_2.ID_3_Motor_send.kp = 0; CAN_2.ID_3_Motor_send.kd = 0; CAN_2.ID_3_Motor_send.speed = 0;
|
||||
CAN_2.ID_4_Motor_send.torque = 0; CAN_2.ID_4_Motor_send.kp = 0; CAN_2.ID_4_Motor_send.kd = 0; CAN_2.ID_4_Motor_send.speed = 0;
|
||||
|
||||
CAN_Send_Control(&hcan1, &CAN_1.ID_1_Motor_send);
|
||||
CAN_Send_Control(&hcan1, &CAN_1.ID_2_Motor_send);
|
||||
CAN_Send_Control(&hcan1, &CAN_1.ID_3_Motor_send);
|
||||
CAN_Send_Control(&hcan1, &CAN_1.ID_4_Motor_send);
|
||||
|
||||
CAN_Send_Control(&hcan2, &CAN_2.ID_1_Motor_send);
|
||||
CAN_Send_Control(&hcan2, &CAN_2.ID_2_Motor_send);
|
||||
CAN_Send_Control(&hcan2, &CAN_2.ID_3_Motor_send);
|
||||
CAN_Send_Control(&hcan2, &CAN_2.ID_4_Motor_send);
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* CAN 接收回调 (原 can_device.c, 合并至此)
|
||||
* ============================================================ */
|
||||
|
||||
/* 速度低通滤波系数: 新值权重 0.25, 保留 75% 历史
|
||||
* 等效时间常数 ≈ 3 × 控制周期 (2ms) = 6ms
|
||||
* 足以抑制 CAN 量化噪声, 同时对步态响应无明显滞后 */
|
||||
#define SPEED_LPF_ALPHA 0.25f
|
||||
|
||||
static void parse_motor_feedback(Motor_CAN_Recieve_Struct *out,
|
||||
uint32_t actual_id, uint8_t data[])
|
||||
{
|
||||
uint32_t motor_id = (actual_id >> 8) & 0xFF;
|
||||
uint32_t master_id = actual_id & 0xFF;
|
||||
uint8_t status = (actual_id >> 16) & 0xFF;
|
||||
|
||||
out->master_id = master_id;
|
||||
out->motor_id = motor_id;
|
||||
|
||||
uint16_t pos_int = (data[0] << 8) | data[1];
|
||||
out->current_position_f = uint_to_float_mit(pos_int, P_MAX, 16);
|
||||
|
||||
/* 速度: 一阶低通滤波, 抑制量化噪声 */
|
||||
uint16_t spd_int = (data[2] << 8) | data[3];
|
||||
float new_speed = uint_to_float_mit(spd_int, V_MAX, 16);
|
||||
out->current_speed_f = SPEED_LPF_ALPHA * new_speed
|
||||
+ (1.0f - SPEED_LPF_ALPHA) * out->current_speed_f;
|
||||
|
||||
uint16_t tor_int = (data[4] << 8) | data[5];
|
||||
out->current_torque_f = uint_to_float_mit(tor_int, T_MAX, 16);
|
||||
|
||||
uint16_t temp_int = (data[6] << 8) | data[7];
|
||||
out->current_temp_f = (float)temp_int * 0.1f;
|
||||
|
||||
out->last_update_time = HAL_GetTick();
|
||||
out->fault_message = status;
|
||||
}
|
||||
|
||||
void can1_recv_callback(uint32_t recv_id, uint8_t data[])
|
||||
{
|
||||
uint32_t actual_id = recv_id;
|
||||
if (hcan1.pRxMsg != NULL && hcan1.pRxMsg->IDE == CAN_ID_EXT)
|
||||
actual_id = hcan1.pRxMsg->ExtId;
|
||||
|
||||
if (((actual_id >> 24) & 0x1F) != 0x2) return;
|
||||
|
||||
parse_motor_feedback(&Motor_Recieve_Single_CAN1, actual_id, data);
|
||||
|
||||
switch (Motor_Recieve_Single_CAN1.motor_id) {
|
||||
case 1: CAN_1.ID_1_Motor_recieve = Motor_Recieve_Single_CAN1; break;
|
||||
case 2: CAN_1.ID_2_Motor_recieve = Motor_Recieve_Single_CAN1; break;
|
||||
case 3: CAN_1.ID_3_Motor_recieve = Motor_Recieve_Single_CAN1; break;
|
||||
case 4: CAN_1.ID_4_Motor_recieve = Motor_Recieve_Single_CAN1; break;
|
||||
}
|
||||
}
|
||||
|
||||
void can2_recv_callback(uint32_t recv_id, uint8_t data[])
|
||||
{
|
||||
uint32_t actual_id = recv_id;
|
||||
if (hcan2.pRxMsg != NULL && hcan2.pRxMsg->IDE == CAN_ID_EXT)
|
||||
actual_id = hcan2.pRxMsg->ExtId;
|
||||
|
||||
if (((actual_id >> 24) & 0x1F) != 0x2) return;
|
||||
|
||||
parse_motor_feedback(&Motor_Recieve_Single_CAN2, actual_id, data);
|
||||
|
||||
switch (Motor_Recieve_Single_CAN2.motor_id) {
|
||||
case 1: CAN_2.ID_1_Motor_recieve = Motor_Recieve_Single_CAN2; break;
|
||||
case 2: CAN_2.ID_2_Motor_recieve = Motor_Recieve_Single_CAN2; break;
|
||||
case 3: CAN_2.ID_3_Motor_recieve = Motor_Recieve_Single_CAN2; break;
|
||||
case 4: CAN_2.ID_4_Motor_recieve = Motor_Recieve_Single_CAN2; break;
|
||||
}
|
||||
}
|
||||
|
||||
/* CAN 总线滤波器初始化 */
|
||||
static CanRxMsgTypeDef Rx1Message;
|
||||
static CanRxMsgTypeDef Rx2Message;
|
||||
|
||||
void can_manual_init(void)
|
||||
{
|
||||
CAN_FilterConfTypeDef f;
|
||||
f.FilterActivation = ENABLE;
|
||||
f.FilterMode = CAN_FILTERMODE_IDMASK;
|
||||
f.FilterScale = CAN_FILTERSCALE_32BIT;
|
||||
f.FilterIdHigh = 0; f.FilterIdLow = 0;
|
||||
f.FilterMaskIdHigh = 0; f.FilterMaskIdLow = 0;
|
||||
f.FilterFIFOAssignment = CAN_FILTER_FIFO0;
|
||||
f.BankNumber = 14;
|
||||
|
||||
f.FilterNumber = 0;
|
||||
HAL_CAN_ConfigFilter(&hcan1, &f);
|
||||
if (hcan1.pRxMsg == NULL) hcan1.pRxMsg = &Rx1Message;
|
||||
HAL_CAN_Receive_IT(&hcan1, CAN_FIFO0);
|
||||
|
||||
f.FilterNumber = 14;
|
||||
f.FilterActivation = DISABLE;
|
||||
HAL_CAN_ConfigFilter(&hcan2, &f);
|
||||
f.FilterActivation = ENABLE;
|
||||
HAL_CAN_ConfigFilter(&hcan2, &f);
|
||||
if (hcan2.pRxMsg == NULL) hcan2.pRxMsg = &Rx2Message;
|
||||
HAL_CAN_Receive_IT(&hcan2, CAN_FIFO0);
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
#ifndef __LINGZU_MOTOR_H__
|
||||
#define __LINGZU_MOTOR_H__
|
||||
|
||||
#include "stm32f4xx_hal.h"
|
||||
#include "can.h" // For CAN_HandleTypeDef
|
||||
|
||||
/* RS02 电机协议范围（私有协议运控模式）
|
||||
* P: 新固件 >= 0.2.2.11 使用 ±12.57
|
||||
* V: RS02 额定 ±44 rad/s
|
||||
* T: RS02 峰值 ±17 Nm
|
||||
* Kp: RS02 最大 500, Kd: RS02 最大 5
|
||||
*/
|
||||
#define P_MIN -12.57f
|
||||
#define P_MAX 12.57f
|
||||
#define V_MIN -44.0f
|
||||
#define V_MAX 44.0f
|
||||
#define KP_MIN 0.0f
|
||||
#define KP_MAX 500.0f
|
||||
#define KD_MIN 0.0f
|
||||
#define KD_MAX 5.0f
|
||||
#define T_MIN -17.0f
|
||||
#define T_MAX 17.0f
|
||||
|
||||
#define KP_SOFTSTOP 400.0f
|
||||
#define KD_SOFTSTOP 3.0f
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t mode;
|
||||
uint16_t exdata;
|
||||
uint8_t res;
|
||||
|
||||
float position;
|
||||
float speed;
|
||||
float kp;
|
||||
float kd;
|
||||
float torque;
|
||||
|
||||
//位置限制
|
||||
float max_position;
|
||||
float min_position;
|
||||
|
||||
} Motor_CAN_Send_Struct;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t master_id;
|
||||
uint8_t motor_id;
|
||||
uint8_t fault_message;
|
||||
uint8_t motor_state;
|
||||
uint8_t mode;
|
||||
|
||||
uint16_t current_position; // [0~65535] (-4π~4π)
|
||||
uint16_t current_speed; // [0~65535] (-15rad/s~15rad/s)
|
||||
uint16_t current_torque; // [0~65535] (-120Nm~120Nm)
|
||||
uint16_t current_temp; // Temp * 10
|
||||
|
||||
float current_position_f;
|
||||
float current_speed_f;
|
||||
float current_torque_f;
|
||||
float current_temp_f;
|
||||
|
||||
uint32_t last_update_time; // Timestamp of last valid feedback
|
||||
|
||||
} Motor_CAN_Recieve_Struct;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Motor_CAN_Send_Struct ID_1_Motor_send, ID_2_Motor_send, ID_3_Motor_send, ID_4_Motor_send;
|
||||
Motor_CAN_Recieve_Struct ID_1_Motor_recieve, ID_2_Motor_recieve, ID_3_Motor_recieve, ID_4_Motor_recieve;
|
||||
} Can_Bus_Data_Struct;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
RS02_MOTOR_1 = 1,
|
||||
RS02_MOTOR_2 = 2,
|
||||
RS02_MOTOR_3 = 3,
|
||||
RS02_MOTOR_4 = 4,
|
||||
RS02_MOTOR_5 = 5,
|
||||
RS02_MOTOR_6 = 6,
|
||||
RS02_MOTOR_7 = 7,
|
||||
RS02_MOTOR_8 = 8
|
||||
} rs02_motor_index_e;
|
||||
|
||||
extern Motor_CAN_Recieve_Struct Motor_Recieve_Single_CAN1;
|
||||
extern Motor_CAN_Recieve_Struct Motor_Recieve_Single_CAN2;
|
||||
extern Can_Bus_Data_Struct CAN_1, CAN_2;
|
||||
|
||||
// Functions
|
||||
void Lingzu_Motor_Init_Structs(void);
|
||||
void Motor_Enable(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data);
|
||||
void Motor_Disable(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data);
|
||||
void Motor_Zore(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data);
|
||||
void CAN_Send_Control(CAN_HandleTypeDef *hcan, Motor_CAN_Send_Struct *Motor_Data);
|
||||
void ENABLE_ALL_LINGZU_MOTORS(void);
|
||||
void DISABLE_ALL_LINGZU_MOTORS(void);
|
||||
void ZERO_ALL_LINGZU_MOTORS(void);
|
||||
|
||||
void LINGZU_All_Motors_Limp(void);
|
||||
|
||||
/**
|
||||
* 输入: motor_index(uint8_t), position(float), speed(float), kp(float), kd(float), torque(float)
|
||||
* 输出: 无
|
||||
* 作用: 设置指定编号RS02电机的目标角度及控制参数
|
||||
*/
|
||||
void rs02_set_target_rad(uint8_t motor_index, float position, float speed, float kp, float kd, float torque);
|
||||
|
||||
/**
|
||||
* 输入: motor_index(uint8_t)
|
||||
* 输出: 当前角度(float, rad)
|
||||
* 作用: 读取指定编号RS02电机的当前关节角度（来自CAN反馈帧）
|
||||
*/
|
||||
float rs02_get_position_rad(uint8_t motor_index);
|
||||
|
||||
/**
|
||||
* 输入: motor_index(uint8_t)
|
||||
* 输出: 当前角速度(float, rad/s)
|
||||
* 作用: 读取指定编号RS02电机的当前角速度（来自CAN反馈帧）
|
||||
*/
|
||||
float rs02_get_velocity_rad(uint8_t motor_index);
|
||||
|
||||
/**
|
||||
* 输入: motor_index(uint8_t), target_position(float)
|
||||
* 输出: 角度误差(float, rad)
|
||||
* 作用: 计算目标角度与当前角度的误差
|
||||
*/
|
||||
float rs02_get_error_rad(uint8_t motor_index, float target_position);
|
||||
|
||||
// Utils
|
||||
float uint_to_float(int x_int, float x_min, float x_max, int bits);
|
||||
int float_to_uint(float x, float x_min, float x_max, int bits);
|
||||
float uint_to_float_mit(int x_int, float limit, int bits);
|
||||
int float_to_uint_mit(float x, float limit, int bits);
|
||||
int float_to_uint_mit_param(float x, float limit);
|
||||
|
||||
// CAN 接收回调 (原 can_device.h)
|
||||
void can1_recv_callback(uint32_t recv_id, uint8_t data[]);
|
||||
void can2_recv_callback(uint32_t recv_id, uint8_t data[]);
|
||||
void can_manual_init(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,138 @@
|
||||
/****************************************************************************
|
||||
* RoboMentors www.robomentors.com.
|
||||
* wechat:superzz8080
|
||||
* 基于RoboMaster二次开发
|
||||
***************************************************************************/
|
||||
|
||||
#include "uart_device.h"
|
||||
#include "sys.h"
|
||||
|
||||
#include "stdlib.h"
|
||||
#include "string.h"
|
||||
|
||||
/* 解析后的遥控器数据 */
|
||||
rc_type_t rc;
|
||||
/* 接收到的遥控器原始数据 */
|
||||
uint8_t dbus_recv[DBUS_FRAME_SIZE];
|
||||
|
||||
/**
|
||||
* @brief 遥控器中断回调函数，在设置 UART 接收时注册
|
||||
*/
|
||||
void dbus_uart_callback(void)
|
||||
{
|
||||
remote_data_handle(&rc, dbus_recv);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 解析遥控器数据
|
||||
* @param rc: 解析后的遥控器数据结构体指针
|
||||
* @param buff: 串口接收到的遥控器原始数据指针
|
||||
*/
|
||||
static void remote_data_handle(rc_type_t *rc, uint8_t *buff)
|
||||
{
|
||||
/* SBUS Start Byte Check */
|
||||
// if (buff[0] != 0x0F) // DJI SBUS uses 0x00? Standard SBUS uses 0x0F.
|
||||
// 暂时移除起始字节检查，防止协议差异导致丢包
|
||||
// {
|
||||
// memset(rc, 0, sizeof(rc_type_t));
|
||||
// return;
|
||||
// }
|
||||
|
||||
/* Parse SBUS Channels (11 bits per channel) */
|
||||
int16_t ch[16];
|
||||
ch[0] = ((buff[1] | buff[2]<<8) & 0x07FF);
|
||||
|
||||
/*
|
||||
* CRITICAL FIX: 过滤全0或极低值的噪声数据
|
||||
* 正常 SBUS 通道值范围通常在 200 到 1800 之间。
|
||||
* 如果接收到 0 或者非常小的值，说明是无效帧（可能是全0噪声）。
|
||||
* 直接丢弃该帧，保留上一次的有效 rc 状态。
|
||||
*/
|
||||
if (ch[0] < 100)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
ch[1] = ((buff[2]>>3 | buff[3]<<5) & 0x07FF);
|
||||
ch[2] = ((buff[3]>>6 | buff[4]<<2 | buff[5]<<10) & 0x07FF);
|
||||
ch[3] = ((buff[5]>>1 | buff[6]<<7) & 0x07FF);
|
||||
ch[4] = ((buff[6]>>4 | buff[7]<<4) & 0x07FF);
|
||||
// ch[5] = ((buff[7]>>7 | buff[8]<<1 | buff[9]<<9) & 0x07FF);
|
||||
ch[6] = ((buff[9]>>2 | buff[10]<<6) & 0x07FF);
|
||||
ch[7] = ((buff[10]>>5 | buff[11]<<3) & 0x07FF);
|
||||
|
||||
/* Corrected SBUS Parsing for Ch9-Ch12 (Indices 8-11) */
|
||||
/* Pattern repeats every 11 bytes (8 channels) */
|
||||
/* ch[8] corresponds to ch[0] pattern, shifted by 11 bytes */
|
||||
ch[8] = ((buff[12] | buff[13]<<8) & 0x07FF);
|
||||
ch[9] = ((buff[13]>>3 | buff[14]<<5) & 0x07FF);
|
||||
// ch[10] = ((buff[14]>>6 | buff[15]<<2 | buff[16]<<10) & 0x07FF);
|
||||
// ch[11] = ((buff[16]>>1 | buff[17]<<7) & 0x07FF);
|
||||
|
||||
/* Map Channels to rc structure
|
||||
RadioLink AT9S Pro (SBUS) mapping:
|
||||
Ch1: Aileron (Right LR) -> rc->ch1
|
||||
Ch2: Elevator (Right UD) -> rc->ch2
|
||||
Ch3: Throttle (Left UD) -> rc->ch4 (Note: rc->ch4 is Left UD in current struct comments)
|
||||
Ch4: Rudder (Left LR) -> rc->ch3 (Note: rc->ch3 is Left LR in current struct comments)
|
||||
*/
|
||||
|
||||
/* Normalize to -660 ~ 660 range (SBUS range approx 200~1800, center 1024) */
|
||||
/* Scaling factor: 660 / (1800-1024) approx 0.85 */
|
||||
|
||||
rc->ch1 = (ch[0] - SBUS_RC_MID) * 660 / 800;
|
||||
rc->ch2 = (ch[1] - SBUS_RC_MID) * 660 / 800;
|
||||
rc->ch3 = (ch[3] - SBUS_RC_MID) * 660 / 800; // Map SBUS Ch4 (Rudder) to rc->ch3
|
||||
rc->ch4 = (ch[2] - SBUS_RC_MID) * 660 / 800; // Map SBUS Ch3 (Throttle) to rc->ch4
|
||||
|
||||
/* 防止遥控器零点有偏差 */
|
||||
if(abs(rc->ch1) < 50) rc->ch1 = 0;
|
||||
if(abs(rc->ch2) < 50) rc->ch2 = 0;
|
||||
if(abs(rc->ch3) < 50) rc->ch3 = 0;
|
||||
if(abs(rc->ch4) < 50) rc->ch4 = 0;
|
||||
|
||||
/* SwC (Ch8) */
|
||||
if (ch[7] < 500) rc->swC = RC_DN;
|
||||
else if (ch[7] > 1500) rc->swC = RC_UP;
|
||||
else rc->swC = RC_MI;
|
||||
|
||||
/* SwA (Ch10) */
|
||||
if (ch[9] < 500) rc->swA = RC_DN;
|
||||
else if (ch[9] > 1500) rc->swA = RC_UP;
|
||||
else rc->swA = RC_MI;
|
||||
|
||||
/* SwD (Ch7) */
|
||||
if (ch[6] < 500) rc->swD = RC_DN;
|
||||
else if (ch[6] > 1500) rc->swD = RC_UP;
|
||||
else rc->swD = RC_MI;
|
||||
|
||||
/* SwG (Ch5) */
|
||||
if (ch[4] < 500) rc->swG = RC_DN;
|
||||
else if (ch[4] > 1500) rc->swG = RC_UP;
|
||||
else rc->swG = RC_MI;
|
||||
|
||||
/*
|
||||
* CRITICAL FIX: 如果遥控器还没开，接收到的数据可能是全0
|
||||
* 这种情况下，ch[4] = 0 -> rc->sw1 = RC_DN
|
||||
* 这会导致系统认为开关处于“向下”状态（趴下模式）。
|
||||
* 但如果此时 LED1 快闪，说明确实收到了数据帧。
|
||||
*
|
||||
* 问题：为什么快闪（有数据）但没反应？
|
||||
* 可能是遥控器通道映射不对，或者开关值反了。
|
||||
*/
|
||||
|
||||
/* 遥控器异常值处理，函数直接返回 */
|
||||
// 暂时放宽限制，防止轻微越界导致丢失控制
|
||||
if ((abs(rc->ch1) > 800) || \
|
||||
(abs(rc->ch2) > 800) || \
|
||||
(abs(rc->ch3) > 800) || \
|
||||
(abs(rc->ch4) > 800))
|
||||
{
|
||||
return ;
|
||||
}
|
||||
|
||||
/* Clear Mouse and Keyboard (Not available in SBUS) - Removed
|
||||
memset(&rc->mouse, 0, sizeof(rc->mouse));
|
||||
memset(&rc->kb, 0, sizeof(rc->kb));
|
||||
*/
|
||||
}
|
||||
@@ -0,0 +1,106 @@
|
||||
/****************************************************************************
|
||||
* RoboMentors www.robomentors.com.
|
||||
* wechat:superzz8080
|
||||
* 基于RoboMaster二次开发
|
||||
***************************************************************************/
|
||||
|
||||
#ifndef __UART_DEVICE_H__
|
||||
#define __UART_DEVICE_H__
|
||||
|
||||
#include "rm_hal_lib.h"
|
||||
|
||||
#define SBUS_RC_MIN 200
|
||||
#define SBUS_RC_MID 1024
|
||||
#define SBUS_RC_MAX 1800
|
||||
|
||||
/**
|
||||
* @brief 解析后的遥控器数据结构体
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
/* 遥控器的通道数据，数值范围：-660 ~ 660 */
|
||||
int16_t ch1; //右侧左右
|
||||
int16_t ch2; //右侧上下
|
||||
int16_t ch3; //左侧左右
|
||||
int16_t ch4; //左侧上下
|
||||
|
||||
/* 遥控器的拨杆数据，上中下分别为：1、3、2 */
|
||||
uint8_t sw1; //SwF (Ch5) - Left Switch
|
||||
uint8_t sw2; //SwG? (Ch6) - Right Switch (Undefined in doc)
|
||||
uint8_t sw3; //SwC (Ch7)
|
||||
uint8_t sw4; //VrB/SwD? (Ch8) - Knob/Switch
|
||||
|
||||
/* 新增辅助通道映射 */
|
||||
uint8_t swA; //SwA (Ch10)
|
||||
uint8_t swB; //SwB (Ch9)
|
||||
uint8_t swC; //SwC (Ch7) - Alias for sw3
|
||||
uint8_t swD; //SwD (Ch7)
|
||||
uint8_t swF; //SwF (Ch5) - Alias for sw1
|
||||
uint8_t swG; //SwG (Ch5)
|
||||
uint8_t swH; //SwH (Ch7)
|
||||
|
||||
/* PC 鼠标数据 */
|
||||
struct
|
||||
{
|
||||
/* 鼠标移动相关 */
|
||||
int16_t x; //鼠标平移
|
||||
int16_t y; //鼠标上下
|
||||
/* 鼠标按键相关，1为按下，0为松开 */
|
||||
uint8_t l; //左侧按键
|
||||
uint8_t r; //右侧按键
|
||||
}mouse;
|
||||
|
||||
/* PC 键盘按键数据 */
|
||||
union
|
||||
{
|
||||
uint16_t key_code;
|
||||
struct
|
||||
{
|
||||
uint16_t W:1;
|
||||
uint16_t S:1;
|
||||
uint16_t A:1;
|
||||
uint16_t D:1;
|
||||
uint16_t SHIFT:1;
|
||||
uint16_t CTRL:1;
|
||||
uint16_t Q:1;
|
||||
uint16_t E:1;
|
||||
uint16_t R:1;
|
||||
uint16_t F:1;
|
||||
uint16_t G:1;
|
||||
uint16_t Z:1;
|
||||
uint16_t X:1;
|
||||
uint16_t C:1;
|
||||
uint16_t V:1;
|
||||
uint16_t B:1;
|
||||
}bit;
|
||||
}kb;
|
||||
|
||||
/* 遥控器左侧拨轮数据 */
|
||||
int16_t wheel;
|
||||
} rc_type_t;
|
||||
|
||||
/**
|
||||
* @brief 遥控器拨杆数据枚举
|
||||
*/
|
||||
enum
|
||||
{
|
||||
RC_UP = 1,
|
||||
RC_MI = 3,
|
||||
RC_DN = 2,
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 解析遥控器数据
|
||||
* @param rc: 解析后的遥控器数据结构体指针
|
||||
* @param buff: 串口接收到的遥控器原始数据指针
|
||||
*/
|
||||
static void remote_data_handle(rc_type_t *rc, uint8_t *buff);
|
||||
/**
|
||||
* @brief 遥控器中断回调函数，在设置 UART 接收时注册
|
||||
*/
|
||||
void dbus_uart_callback(void);
|
||||
|
||||
extern rc_type_t rc;
|
||||
extern uint8_t dbus_recv[];
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,326 @@
|
||||
/****************************************************************************
|
||||
* RoboMentors www.robomentors.com.
|
||||
* wechat:superzz8080
|
||||
* 基于RoboMaster二次开发
|
||||
***************************************************************************/
|
||||
|
||||
#ifndef __RM_LIB_H__
|
||||
#define __RM_LIB_H__
|
||||
|
||||
#include "stm32f4xx_hal.h"
|
||||
|
||||
/**
|
||||
* @brief IMU 数据结构体
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
float acc_x; //m/s^2
|
||||
float acc_y; //m/s^2
|
||||
float acc_z; //m/s^2
|
||||
float gyro_x; //degree/s
|
||||
float gyro_y; //degree/s
|
||||
float gyro_z; //degree/s
|
||||
float angle_x; //degree
|
||||
float angle_y; //degree
|
||||
float angle_z; //degree
|
||||
} imu_t;
|
||||
|
||||
/**
|
||||
* @brief UART 配置类型枚举
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
WORD_LEN_8B = 0,
|
||||
WORD_LEN_9B,
|
||||
STOP_BITS_1,
|
||||
STOP_BITS_2,
|
||||
PARITY_NONE,
|
||||
PARITY_EVEN,
|
||||
PARITY_ODD,
|
||||
} uart_config_e;
|
||||
|
||||
/**
|
||||
* @brief 数字 IO 引脚方向枚举
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
IO_INPUT = 1,
|
||||
IO_OUTPUT = 2,
|
||||
} digital_io_e;
|
||||
|
||||
/**
|
||||
* @brief LED IO 状态
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
LED_ON = 0,
|
||||
LED_OFF = 1,
|
||||
} led_io_e;
|
||||
|
||||
//CAN 设备端口定义
|
||||
#define USER_CAN1 1 //CAN1
|
||||
#define USER_CAN2 2 //CAN2
|
||||
#define CHASSIS_CAN 1 //底盘电机使用CAN1
|
||||
#define GIMBAL_CAN 1 //云台电机使用CAN1
|
||||
#define TRIGGER_CAN 1 //拨弹电机使用CAN1
|
||||
//CAN 设备相关函数
|
||||
/**
|
||||
* @brief CAN 设备初始化
|
||||
*/
|
||||
void can_device_init(void);
|
||||
/**
|
||||
* @brief 发送 CAN 数据
|
||||
* @param can_id: CAN 设备 ID，只有 CAN1 或者 CAN2
|
||||
* @param send_id: 发送数据 ID
|
||||
* @param send_data: 发送数据指针，大小为 8 位
|
||||
*/
|
||||
void write_can(uint8_t can_id, uint32_t send_id, uint8_t send_data[]);
|
||||
/**
|
||||
* @brief 注册 CAN 接收回调函数
|
||||
* @param can_id: CAN 设备 ID
|
||||
* @param callback: 接收回调函数指针
|
||||
*/
|
||||
void can_recv_callback_register(uint8_t can_id, void (* callback)(uint32_t recv_id, uint8_t data[]));
|
||||
/**
|
||||
* @brief 开启 CAN 接收数据中断
|
||||
*/
|
||||
void can_receive_start(void);
|
||||
|
||||
//UART 设备端口定义
|
||||
#define DBUS_UART 1 //遥控器接收机串口
|
||||
#define USER_UART1 1 //用户串口1，遥控器接收机使用
|
||||
#define USER_UART2 2 //用户串口2
|
||||
#define USER_UART3 3 //用户串口3
|
||||
#define USER_UART4 4 //用户串口4
|
||||
#define USER_UART5 5 //用户串口5，用于和官方上位机通信
|
||||
//UART 设备相关函数
|
||||
/**
|
||||
* @brief UART 设备初始化
|
||||
* @param uart_id: UART 设备 ID
|
||||
* @param baud_rate: 波特率
|
||||
* @param word_len: 字长，数据类型为 uart_config_e
|
||||
* @param stop_bits: 停止位，数据类型为 uart_config_e
|
||||
* @param parity: 校验位，数据类型为 uart_config_e
|
||||
*/
|
||||
void uart_init(uint8_t uart_id, uint32_t baud_rate, uart_config_e word_len, \
|
||||
uart_config_e stop_bits, uart_config_e parity);
|
||||
/**
|
||||
* @brief 发送 UART 数据
|
||||
* @param uart_id: UART ID
|
||||
* @param send_data: 发送数据指针
|
||||
* @param size: 发送数据的长度
|
||||
*/
|
||||
void write_uart(uint8_t uart_id, uint8_t *send_data, uint16_t size);
|
||||
/**
|
||||
* @brief 注册 UART 接收回调函数
|
||||
* @param uart_id: UART ID
|
||||
* @param callback: 接收回调函数指针
|
||||
*/
|
||||
void uart_recv_callback_register(uint8_t uart_id, void (* callback)(void));
|
||||
/**
|
||||
* @brief 开启 UART 接收数据中断
|
||||
* @param uart_id: UART ID
|
||||
* @param recv_data: 要接收数据的指针
|
||||
* @param size: 要接收的数据的长度
|
||||
*/
|
||||
void uart_receive_start(uint8_t uart_id, uint8_t *recv_data, uint16_t size);
|
||||
|
||||
|
||||
//数字 IO 接口定义
|
||||
#define DIGI_IO1 1
|
||||
#define DIGI_IO2 2
|
||||
#define DIGI_IO3 3
|
||||
#define DIGI_IO4 4
|
||||
#define DIGI_IO5 5
|
||||
#define DIGI_IO6 6
|
||||
#define DIGI_IO7 7
|
||||
#define DIGI_IO8 8
|
||||
#define DIGI_IO9 9
|
||||
//数字 IO 相关函数
|
||||
/**
|
||||
* @brief 设置数字 IO 方向
|
||||
* @param io_id: 数字 IO 的 ID
|
||||
* @param io_type: IO 方向，数据类型为 digital_io_e
|
||||
*/
|
||||
void set_digital_io_dir(uint8_t io_id, digital_io_e io_type);
|
||||
/**
|
||||
* @brief 写数字 IO
|
||||
* @param io_id: 数字 IO 的 ID
|
||||
* @param value: 写入的数据值，只能为0或1，否则设置不成功
|
||||
*/
|
||||
void write_digital_io(uint8_t io_id, uint8_t value);
|
||||
/**
|
||||
* @brief 读数字 IO
|
||||
* @param io_id: 数字 IO 的 ID
|
||||
* @param value: 读取数据值的指针，读取的数据值为0或1
|
||||
*/
|
||||
void read_digital_io(uint8_t io_id, uint8_t *value);
|
||||
|
||||
//LED IO 接口定义
|
||||
#define LED_IO1 1 //LED1 IO
|
||||
#define LED_IO2 2 //LED2 IO
|
||||
#define LED_IO3 3 //LED3 IO
|
||||
#define LED_IO4 4 //LED4 IO
|
||||
#define LED_IO5 5 //LED5 IO
|
||||
#define LED_IO6 6 //LED6 IO
|
||||
#define LED_IO7 7 //LED7 IO
|
||||
#define LED_IO8 8 //LED8 IO
|
||||
#define LED_G 9 //绿色 LED IO
|
||||
#define LED_R 10 //红色 LED IO
|
||||
#define LASER_IO 11 //激光 IO
|
||||
//LED IO 相关函数
|
||||
/**
|
||||
* @brief 写 LED IO
|
||||
* @param led_id: LED IO 的 ID
|
||||
* @param value: 写入的数据值，数据类型为 led_io_e，状态为亮或灭
|
||||
*/
|
||||
void write_led_io(uint8_t led_id, led_io_e value);
|
||||
|
||||
//PWM IO 接口定义
|
||||
#define PWM_GROUP1 1 //第一组 PWM IO
|
||||
#define PWM_IO1 1
|
||||
#define PWM_IO2 2
|
||||
#define PWM_IO3 3
|
||||
#define PWM_IO4 4
|
||||
|
||||
#define PWM_GROUP2 2 //第二组 PWM IO
|
||||
#define PWM_IO5 5
|
||||
#define PWM_IO6 6
|
||||
#define PWM_IO7 7
|
||||
#define PWM_IO8 8
|
||||
|
||||
#define PWM_GROUP3 3 //第三组 PWM IO
|
||||
#define PWM_IO9 9
|
||||
#define PWM_IO10 10
|
||||
#define PWM_IO11 11
|
||||
#define PWM_IO12 12
|
||||
|
||||
#define PWM_GROUP4 4 //第四组 PWM IO
|
||||
#define PWM_IO13 13
|
||||
#define PWM_IO14 14
|
||||
#define PWM_IO15 15
|
||||
#define PWM_IO16 16
|
||||
//PWM IO 相关函数
|
||||
/**
|
||||
* @brief 开启 PWM 输出
|
||||
* @param pwm_id: PWM IO 的 ID
|
||||
*/
|
||||
void start_pwm_output(uint8_t pwm_id);
|
||||
/**
|
||||
* @brief 设置 PWM 组对应参数
|
||||
* @param pwm_group: PWM 组别，目前有 4 组PWM
|
||||
* @param period: 每组 PWM 对应周期时间，单位是微秒(us)
|
||||
*/
|
||||
void set_pwm_group_param(uint8_t pwm_group, uint32_t period);
|
||||
/**
|
||||
* @brief 设置 PWM IO 参数
|
||||
* @param pwm_id: PWM IO 的 ID
|
||||
* @param pulse: 配置 PWM 的高电平时间，单位是微秒(us)
|
||||
*/
|
||||
void set_pwm_param(uint8_t pwm_id, uint32_t pulse);
|
||||
|
||||
|
||||
//蜂鸣器 IO
|
||||
/* 板子上只有一个蜂鸣器 */
|
||||
#define BEEP1_IO 1 //蜂鸣器1 IO
|
||||
|
||||
#define BEEP_FREQ 3000 //默认蜂鸣器频率
|
||||
#define BEEP_ON 1 //打开蜂鸣器
|
||||
#define BEEP_OFF 0 //关闭蜂鸣器
|
||||
//蜂鸣器 IO 相关函数
|
||||
/**
|
||||
* @brief 设置蜂鸣器 IO 参数
|
||||
* @param beep_id: 蜂鸣器的 ID
|
||||
* @param freq: 配置蜂鸣器响的声音的频率(Hz)
|
||||
* @param ctrl: 蜂鸣器开启控制，参数可以为BEEP_ON/BEEP_OFF
|
||||
*/
|
||||
void set_beep_param(uint8_t beep_id, uint32_t freq, uint8_t ctrl);
|
||||
|
||||
|
||||
//按键 IO 接口定义
|
||||
#define KEY_IO1 1 //用户按键1
|
||||
#define KEY_IO2 2 //用户按键2
|
||||
#define KEY_IO3 3 //用户按键3
|
||||
#define KEY_IO4 4 //用户按键4
|
||||
//按键 IO 相关函数
|
||||
/**
|
||||
* @brief 读按键 IO
|
||||
* @param key_id: 按键 IO 的 ID
|
||||
* @param value: 读取数据值的指针，读取的数据值为0或1，按键没有按下时为1，按下为0
|
||||
*/
|
||||
void read_key_io(uint8_t key_id, uint8_t *value);
|
||||
|
||||
|
||||
//ADC IO 接口定义
|
||||
#define ADC_IO1 1 //ADC接口1
|
||||
//ADC IO 相关函数
|
||||
/**
|
||||
* @brief 读 ADC IO 数据
|
||||
* @param adc_id: ADC IO 的 ID
|
||||
* @param value: 读取数据值的指针，读取的数据值为 uint32_t 类型的正整数，
|
||||
* 数据范围为 0 ~ 65535
|
||||
* 目前只有一个 ADC 接口，用于拓展版上的霍尔传感器
|
||||
*/
|
||||
void read_adc_io(uint8_t adc_id, uint32_t *value);
|
||||
|
||||
|
||||
//数码管接口定义
|
||||
#define DIG_TUBE1 1
|
||||
#define DIG_TUBE2 2
|
||||
#define DIG_TUBE3 3
|
||||
#define DIG_TUBE4 4
|
||||
#define DIG_TUBE5 5
|
||||
#define DIG_TUBE6 6
|
||||
#define DIG_TUBE7 7
|
||||
#define DIG_TUBE8 8
|
||||
|
||||
#define TUBE_ALL_ON 255 //数码管全亮
|
||||
#define TUBE_ALL_OFF 0 //数码管全灭
|
||||
//数码管接口相关函数
|
||||
/**
|
||||
* @brief 数码管初始化
|
||||
* @usage 在使用数码管显示前使用，配置数码管各个控制引脚的功能，
|
||||
* @attention 因为数码管控制接口和用户接口复用，在使用数码管时 9 个用户数字 IO 不能再被使用
|
||||
*/
|
||||
void digital_tube_init(void);
|
||||
/**
|
||||
* @brief 刷新所有数码管
|
||||
* @usage 数码管需要显示新的数据时使用
|
||||
*/
|
||||
void refresh_digital_tube(void);
|
||||
/**
|
||||
* @brief 第几个数码管显示具体数字
|
||||
* @param pos: 从左到右第几个数码管
|
||||
* @param code: 数码管需要显示的数据，数据类型为 uint8_t 用 10 进制表示的范围为 0 ~ 255
|
||||
* 8位数据由高到低分别对应每段的：A B C D E F G P
|
||||
* 数码管的段和数据代码的对应位置1即可点亮，例如显示数字1，数据代码为96
|
||||
* 每位的具体位置见说明书，其中 P 为小数点
|
||||
*/
|
||||
void switch_display_num(uint8_t pos, uint8_t code);
|
||||
|
||||
|
||||
|
||||
//其他外设接口相关函数
|
||||
//flash
|
||||
/**
|
||||
* @brief 写 FLASH 设备
|
||||
* @param write_data: 写入数据的指针
|
||||
* @param len: 写入数据的长度
|
||||
*/
|
||||
void write_flash(uint8_t *write_data, uint32_t len);
|
||||
/**
|
||||
* @brief 读 FLASH 设备
|
||||
* @param read_data: 读取数据的指针
|
||||
* @param len: 读取数据的长度
|
||||
*/
|
||||
void read_flash(uint8_t *read_data, uint32_t len);
|
||||
|
||||
//imu
|
||||
/**
|
||||
* @brief 读取 IMU 数据
|
||||
* @param imu_data: 接收 IMU 数据的结构体指针
|
||||
* @usage 需要在循环任务中调用，用来刷新 IMU 数据
|
||||
*/
|
||||
void get_imu_data(imu_t *imu_data);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,88 @@
|
||||
/****************************************************************************
|
||||
* RoboMentors www.robomentors.com.
|
||||
* wechat:superzz8080
|
||||
* 基于RoboMaster二次开发
|
||||
***************************************************************************/
|
||||
|
||||
#include "startup.h"
|
||||
#include "sys.h"
|
||||
#include "uart_device.h"
|
||||
#include "lingzu_motor.h"
|
||||
#include "uart_task.h"
|
||||
|
||||
|
||||
//运行在定义的任务函数执行前，可以用来初始化任务中用到的 IO 端口，配置、开启外界硬件设备，注册硬件设备的接收回调函数
|
||||
void init_setup(void){
|
||||
|
||||
//关闭所有LED灯
|
||||
write_led_io(LED_IO1, LED_OFF);
|
||||
write_led_io(LED_IO2, LED_OFF);
|
||||
write_led_io(LED_IO3, LED_OFF);
|
||||
write_led_io(LED_IO4, LED_OFF);
|
||||
write_led_io(LED_IO5, LED_OFF);
|
||||
write_led_io(LED_IO6, LED_OFF);
|
||||
write_led_io(LED_IO7, LED_OFF);
|
||||
write_led_io(LED_IO8, LED_OFF);
|
||||
|
||||
digital_tube_init();
|
||||
|
||||
HAL_Delay(2000);
|
||||
|
||||
//初始化遥控器接收串口
|
||||
uart_init(DBUS_UART, 100000, WORD_LEN_8B, STOP_BITS_2, PARITY_EVEN);
|
||||
|
||||
//注册遥控器接收数据回调函数
|
||||
uart_recv_callback_register(DBUS_UART, dbus_uart_callback);
|
||||
|
||||
//开启遥控器接收
|
||||
uart_receive_start(DBUS_UART, dbus_recv, DBUS_FRAME_SIZE);
|
||||
|
||||
//初始化 CAN 设备
|
||||
can_device_init();
|
||||
can_manual_init();
|
||||
|
||||
// 初始化灵足电机数据结构
|
||||
Lingzu_Motor_Init_Structs();
|
||||
|
||||
//注册CAN的回调函数
|
||||
can_recv_callback_register(USER_CAN1, can1_recv_callback);
|
||||
can_recv_callback_register(USER_CAN2, can2_recv_callback);
|
||||
|
||||
//开启CAN接收数据中断
|
||||
can_receive_start();
|
||||
}
|
||||
|
||||
//开启相关任务函数
|
||||
extern TaskHandle_t task1_t;
|
||||
extern TaskHandle_t task2_t;
|
||||
extern TaskHandle_t task3_t;
|
||||
extern TaskHandle_t task4_t;
|
||||
extern TaskHandle_t task5_t;
|
||||
|
||||
void sys_start_task(void){
|
||||
|
||||
#ifdef USER_TASK1
|
||||
osThreadDef(ostask1, USER_TASK1, osPriorityAboveNormal, 0, 1024);
|
||||
task1_t = osThreadCreate(osThread(ostask1), NULL);
|
||||
#endif
|
||||
|
||||
#ifdef USER_TASK2
|
||||
osThreadDef(ostask2, USER_TASK2, osPriorityAboveNormal, 0, 128);
|
||||
task2_t = osThreadCreate(osThread(ostask2), NULL);
|
||||
#endif
|
||||
|
||||
#ifdef USER_TASK3
|
||||
osThreadDef(ostask3, USER_TASK3, osPriorityHigh, 0, 128);
|
||||
task3_t = osThreadCreate(osThread(ostask3), NULL);
|
||||
#endif
|
||||
|
||||
#ifdef USER_TASK4
|
||||
osThreadDef(ostask4, USER_TASK4, osPriorityNormal, 0, 512);
|
||||
task4_t = osThreadCreate(osThread(ostask4), NULL);
|
||||
#endif
|
||||
|
||||
#ifdef USER_TASK5
|
||||
osThreadDef(ostask5, USER_TASK5, osPriorityNormal, 0, 128);
|
||||
task5_t = osThreadCreate(osThread(ostask5), NULL);
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
/****************************************************************************
|
||||
* Copyright (C) 2018 RoboMaster.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
***************************************************************************/
|
||||
|
||||
/**
|
||||
*********************** (C) COPYRIGHT 2018 DJI **********************
|
||||
* @update
|
||||
* @history
|
||||
* Version Date Author Modification
|
||||
* V1.0.0 January-15-2018 ric.luo
|
||||
* @verbatim
|
||||
*********************** (C) COPYRIGHT 2018 DJI **********************
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef __START_UP_H__
|
||||
#define __START_UP_H__
|
||||
|
||||
#include "rm_hal_lib.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "lingzu_task.h"
|
||||
#include "uart_task.h"
|
||||
/**
|
||||
* @brief 最多支持 5 个任务函数的配置和开启
|
||||
* @usage 首先开启 USER_TASKx 的宏定义，然后在 USER_TASKx 后添加需要开启的任务函数名
|
||||
*/
|
||||
#define USER_TASK1 lingzu_task
|
||||
//#define USER_TASK2
|
||||
//#define USER_TASK3
|
||||
#define USER_TASK4 uart_task
|
||||
//#define USER_TASK5
|
||||
|
||||
/**
|
||||
* @brief 在任务函数执行前运行，可以用来初始化任务中用到的 IO 端口，
|
||||
* 配置、开启外界硬件设备，注册硬件设备的接收回调函数
|
||||
*/
|
||||
void init_setup(void);
|
||||
|
||||
/**
|
||||
* @brief 开启任务相关函数，在这里定义不需要改动
|
||||
*/
|
||||
void sys_start_task(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,35 @@
|
||||
/**
|
||||
* @file sys.h
|
||||
* @brief 四足机器人系统配置 (精简版)
|
||||
*
|
||||
* 原文件来自 RoboMaster 步兵车模板, 包含大量底盘/云台/发射参数。
|
||||
* 已删除所有与四足无关的内容, 仅保留系统级必要宏定义。
|
||||
*/
|
||||
|
||||
#ifndef __SYS_H__
|
||||
#define __SYS_H__
|
||||
|
||||
#include "rm_hal_lib.h"
|
||||
|
||||
/* ============================================================
|
||||
* 串口 / DBUS 配置
|
||||
* ============================================================ */
|
||||
#define MAX_DMA_COUNT 200
|
||||
#define DBUS_FRAME_SIZE 25
|
||||
|
||||
/* ============================================================
|
||||
* 通用工具宏
|
||||
* ============================================================ */
|
||||
|
||||
/** @brief 极值限制 */
|
||||
#define VAL_LIMIT(val, min, max) \
|
||||
if((val) <= (min)) \
|
||||
{ \
|
||||
(val) = (min); \
|
||||
} \
|
||||
else if((val) >= (max)) \
|
||||
{ \
|
||||
(val) = (max); \
|
||||
}
|
||||
|
||||
#endif /* __SYS_H__ */
|
||||
Reference in New Issue
Block a user