SkyLiCoder
e1a01e6b53
- host 0xA9 frame → master_machine decode → robot_cmd publish via gimbal_feedforward topic → gimbal subscribe → yaw_motor.feed_input - add DM_Feedforward_Controller_s with Kp and feed_input fields - add feedforward term (Kp * feed_input) to motor control output - add DM_Feedforward_Kp to motor init config
105 lines
3.1 KiB
C
105 lines
3.1 KiB
C
#ifndef MASTER_MACHINE_H
|
|
#define MASTER_MACHINE_H
|
|
|
|
#include "bsp_usart.h"
|
|
#include "bsp_dwt.h"
|
|
#include "bsp_log.h"
|
|
#include "usart.h"
|
|
#include "daemon.h"
|
|
#include <string.h>
|
|
|
|
/*
|
|
* 上位机串口协议 — 四种帧类型, 通过帧头区分
|
|
*
|
|
* ---- 自动/视觉追踪数据帧 0xA5 (18 字节) ----
|
|
* [SOF:0xA5][measure_x:4][ref_x:4][measure_y:4][ref_y:4][checksum:1]
|
|
* measure_x/y: float, 视觉检测目标位置 (测量值)
|
|
* ref_x/y: float, 期望目标位置 (参考值)
|
|
*
|
|
* ---- 手动控制数据帧 0xA6 (10 字节) ----
|
|
* [SOF:0xA6][yaw_velocity:4][pitch_velocity:4][checksum:1]
|
|
* yaw_velocity/pitch_velocity: float, 角速度 (degrees/s)
|
|
*
|
|
* ---- 命令帧 0xA7 (3 字节) ----
|
|
* [SOF:0xA7][cmd:1][checksum:1]
|
|
* cmd: 0x01=自动模式, 0x02=手动模式, 0x03=激光开, 0x04=激光关, 0x05=调试模式
|
|
*
|
|
* ---- 调试调参帧 0xA8 (50 字节) ----
|
|
* [SOF:0xA8][vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
|
|
* [vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
|
|
* [motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
|
|
* [motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4][checksum:1]
|
|
* 全部 float, 依次为视觉环 yaw/pitch 和电机角度环 yaw/pitch 的 PID 参数
|
|
*
|
|
* ---- 前馈输入帧 0xA9 (6 字节) ----
|
|
* [SOF:0xA9][feedforward:4][checksum:1]
|
|
* feedforward: float, 前馈输入值
|
|
*
|
|
* checksum: 帧头到数据末尾前一字节累加和取低8位
|
|
*/
|
|
|
|
#define VISION_SOF_AUTO 0xA5
|
|
#define VISION_SOF_MANUAL 0xA6
|
|
#define VISION_SOF_CMD 0xA7
|
|
#define VISION_SOF_DEBUG 0xA8
|
|
#define VISION_SOF_FEEDFORWARD 0xA9
|
|
|
|
#define VISION_AUTO_LEN 18
|
|
#define VISION_MANUAL_LEN 10
|
|
#define VISION_CMD_LEN 3
|
|
#define VISION_DEBUG_LEN 50
|
|
#define VISION_FEEDFORWARD_LEN 6
|
|
|
|
/* 命令码 */
|
|
#define VISION_CMD_SWITCH_AUTO 0x01
|
|
#define VISION_CMD_SWITCH_MANUAL 0x02
|
|
#define VISION_CMD_LASER_ON 0x03
|
|
#define VISION_CMD_LASER_OFF 0x04
|
|
#define VISION_CMD_SWITCH_DEBUG 0x05
|
|
|
|
/* 工作模式 */
|
|
typedef enum
|
|
{
|
|
VISION_MODE_AUTO = 0, // 自动/视觉追踪
|
|
VISION_MODE_MANUAL = 1, // 手动
|
|
VISION_MODE_DEBUG = 2, // 调试
|
|
} Vision_Mode_e;
|
|
|
|
typedef struct
|
|
{
|
|
/* 自动模式数据 */
|
|
float measure_x;
|
|
float ref_x;
|
|
float measure_y;
|
|
float ref_y;
|
|
uint8_t auto_updated; // 收到新的自动数据帧标志
|
|
|
|
/* 手动模式数据 */
|
|
float yaw_degree_velocity;
|
|
float pitch_degree_velocity;
|
|
uint8_t manual_updated; // 收到新的手动数据帧标志
|
|
|
|
/* 调试模式数据 */
|
|
float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
|
|
float vision_pitch_Kp, vision_pitch_Ki, vision_pitch_Kd;
|
|
float motor_yaw_Kp, motor_yaw_Ki, motor_yaw_Kd;
|
|
float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
|
|
uint8_t debug_updated;
|
|
|
|
/* 前馈输入 */
|
|
float feedforward;
|
|
uint8_t feedforward_updated;
|
|
|
|
/* 命令 */
|
|
uint8_t cmd; // 最新命令码
|
|
uint8_t cmd_pending; // 有待处理命令标志
|
|
|
|
float frequency; // 接收频率 (Hz)
|
|
} VisionData_t;
|
|
|
|
extern VisionData_t vision_data;
|
|
|
|
void Vision_Uart_Init(UART_HandleTypeDef *_handle);
|
|
|
|
#endif // MASTER_MACHINE_H
|