| 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162 |
- /**
- ******************************************************************************
- * @file : heading_diff_ctrl_c.cpp
- * @author : wangyingjie
- * @brief : None
- * @attention : None
- * @date : 2025/5/23
- ******************************************************************************
- */
- #ifndef HEADING_DIFF_CTRL_H
- #define HEADING_DIFF_CTRL_H
- //#ifdef __cplusplus
- //extern "C"
- //{
- //#endif
- #include "control/pid/studio_pid_c.h"
- #include "geography/studio_proj_c.h"
- #include "geometry/studio_geo_algo_c.h"
- //#ifdef __cplusplus
- //}
- //#endif
- // 测试使用
- //#include "geometry/studio_geo_utils.h"
- //#include "geometry/studio_geo_algo.h"
- // 测试使用结束
- // 小船控制结构体
- typedef struct
- {
- studio_pid heading_pid; // 方位角PID控制器
- studio_point_c target; // 目标点
- double base_throttle; // 基础油门
- double left_throttle; // 左侧油门
- double right_throttle; // 右侧油门
- } diff_ctrl;
- // 初始化小船控制器
- void init_diff_ctrl(diff_ctrl *controller,
- const studio_point_c *target,
- double base_throttle, // 输入范围0~1.0
- double Kp,
- double Ki,
- double Kd);
- void reset_diff_ctrl_goal(diff_ctrl *controller, const studio_point_c *target);
- // 正确计算最短路径误差（返回-180~180°）
- double calculate_heading_error(double target_deg, double current_deg);
- double update_boat(diff_ctrl *controller, const studio_point_c *current_pos, double current_heading_deg);
- void simulate_movement(studio_point_c *current_pos, double *current_heading, double left_throttle, double right_throttle);
- #endif // HEADING_DIFF_CTRL_H
|
