Compare commits
6 Commits
a1084cd371
...
2ad0d97189
| Author | SHA1 | Date | |
|---|---|---|---|
| 2ad0d97189 | |||
| 9a2ff3e150 | |||
| ead6c6e64e | |||
| b78aa02d9b | |||
| f121d4057a | |||
| dbf000d55d |
@ -1,26 +1,29 @@
|
||||
|
||||
|
||||
OpenDocument="rc.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/rc.c", Line=31
|
||||
OpenDocument="atti_esti.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/atti_esti.c", Line=0
|
||||
OpenDocument="cmd.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/cmd.c", Line=0
|
||||
GraphedExpression="temp_for_cap[0]", Color=#e56a6f, Show=0
|
||||
GraphedExpression="temp_for_cap[1]", Color=#35792b, Show=0
|
||||
OpenDocument="ai.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/ai.c", Line=24
|
||||
OpenDocument="gimbal.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/gimbal.c", Line=0
|
||||
OpenDocument="port.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c", Line=407
|
||||
OpenDocument="tasks.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3640
|
||||
OpenDocument="can.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/bsp/can.c", Line=380
|
||||
OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32CubeMX/103/Er(Sentry)/startup_stm32f407xx.s", Line=51
|
||||
OpenDocument="chassis.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/chassis.c", Line=0
|
||||
OpenDocument="ai.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/ai.c", Line=0
|
||||
OpenDocument="main.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/Core/Src/main.c", Line=62
|
||||
OpenDocument="supercap.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/device/supercap.c", Line=0
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=273, h=577, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=1, w=738, h=315, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=273, h=114, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=0, w=738, h=376, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=422, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1497, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1295;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1294;0", CodeGraphLegendShown=1, CodeGraphLegendPosition="1253;4"
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=273, h=566, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=1, w=738, h=351, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=273, h=125, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=0, w=738, h=340, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=575, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1344, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="997;1", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1141;0", CodeGraphLegendShown=1, CodeGraphLegendPosition="1100;0"
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[210;144;100;263]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[191;100;100;100;931]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[100;100;100;100;100;100;100;107;107]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" temp_for_cap[0]";" temp_for_cap[1]"], ColWidths=[100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[100;100;126;126;126;126;100;107;107]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;259]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[210;144;100;263]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
||||
WatchedExpression="shoot_mcu_package", RefreshRate=5, Window=Watched Data 1
|
||||
@ -31,4 +34,7 @@ WatchedExpression="cmd_chassis", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="rc_c", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="imu_eulr", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ai_result", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="et16s", Window=Watched Data 1
|
||||
WatchedExpression="et16s", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="temp_for_cap", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="CAN_SuperCapRXData", Window=Watched Data 1
|
||||
WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1
|
||||
@ -2,6 +2,41 @@
|
||||
#include "device/device.h"
|
||||
#include "bsp/uart.h"
|
||||
#include "component/crc16.h"
|
||||
#include "device/referee.h"
|
||||
|
||||
static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
|
||||
uint16_t *crc = NULL;
|
||||
|
||||
if (raw == NULL || len <= sizeof(uint16_t)) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
raw[0] = id;
|
||||
crc = (uint16_t *)(void *)(raw + len - sizeof(uint16_t));
|
||||
*crc = CRC16_Calc(raw, len - sizeof(uint16_t), CRC16_INIT);
|
||||
|
||||
if (CRC16_Verify(raw, len) != true) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
static int8_t Package_Check(const uint8_t *raw, uint16_t len, uint8_t expected_id) {
|
||||
if (raw == NULL || len <= sizeof(uint16_t)) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
if (raw[0] != expected_id) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
if (CRC16_Verify(raw, len) != true) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
|
||||
int8_t AI_StartReceiving(PackageAI_t *ai) {
|
||||
@ -11,10 +46,14 @@ int8_t AI_StartReceiving(PackageAI_t *ai) {
|
||||
}
|
||||
|
||||
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {
|
||||
if (ai == NULL || result == NULL) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
if (Package_Check((const uint8_t *)ai, sizeof(*ai), ID_AI) != DEVICE_OK) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
if(ai->id==ID_AI){
|
||||
// if(CRC16_Verify((const uint8_t*)&(ai), sizeof(&ai))==true)
|
||||
// {
|
||||
result->mode=ai->data.mode;
|
||||
result->gimbal_t.setpoint.yaw=ai->data.yaw;
|
||||
result->gimbal_t.vel.yaw=ai->data.yaw_vel;
|
||||
@ -26,17 +65,18 @@ int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {
|
||||
result->chassis_t.Vx=ai->data.vx;
|
||||
result->chassis_t.Vy=ai->data.vy;
|
||||
result->chassis_t.Vw=ai->data.wz;
|
||||
// }
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
|
||||
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat ){
|
||||
(void)quat;
|
||||
if (mcu == NULL || motor == NULL) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
mcu->id=ID_MCU;
|
||||
mcu->data.mode=0;
|
||||
mcu->data.mode=1;
|
||||
mcu->data.q[0]=motor->imu.quat.q0;
|
||||
mcu->data.q[1]=motor->imu.quat.q1;
|
||||
mcu->data.q[2]=motor->imu.quat.q2;
|
||||
@ -48,16 +88,36 @@ int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* qu
|
||||
mcu->data.bullet_count=0;
|
||||
mcu->data.bullet_speed=22;
|
||||
|
||||
mcu->crc16=CRC16_Calc(((const uint8_t*)&(mcu)),sizeof(&mcu)-sizeof(uint16_t), CRC16_INIT );
|
||||
if(CRC16_Verify(((const uint8_t*)&(mcu)), sizeof(&mcu))!=true){
|
||||
return Package_BuildAndVerify((uint8_t *)mcu, sizeof(*mcu), ID_MCU);
|
||||
}
|
||||
|
||||
int8_t REF_Send(PackageReferee_t *referee,Referee_RobotStatus_t* robot_status,Referee_GameStatus_t *game_status) {
|
||||
if (referee == NULL || robot_status == NULL || game_status == NULL) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
|
||||
referee->data.remain_hp=robot_status->current_HP;
|
||||
referee->data.game_progress = game_status->game_progress & 0x0F;
|
||||
referee->data.stage_remain_time= game_status->stage_remain_time;
|
||||
|
||||
return Package_BuildAndVerify((uint8_t *)referee, sizeof(*referee), ID_REF);
|
||||
}
|
||||
|
||||
int8_t REF_StartSend(PackageReferee_t *referee) {
|
||||
if (referee == NULL) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)referee, sizeof(*referee), true)==HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
|
||||
int8_t MCU_StartSend(PackageMCU_t *mcu) {
|
||||
if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true)==HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -9,6 +9,7 @@ extern "C" {
|
||||
#include "component\user_math.h"
|
||||
#include "remote_control.h"
|
||||
#include "module/gimbal.h"
|
||||
#include "device/referee.h"
|
||||
#include "stdint.h"
|
||||
// struct __attribute__((packed)) GimbalToVision
|
||||
// {
|
||||
@ -162,6 +163,8 @@ typedef struct __attribute__((packed)) {
|
||||
|
||||
|
||||
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat);
|
||||
int8_t REF_Send(PackageReferee_t *referee,Referee_RobotStatus_t* robot_status,Referee_GameStatus_t *game_status);
|
||||
int8_t REF_StartSend(PackageReferee_t *referee);
|
||||
int8_t MCU_StartSend(PackageMCU_t *mcu);
|
||||
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result);
|
||||
int8_t AI_StartReceiving(PackageAI_t *ai);
|
||||
|
||||
@ -614,6 +614,8 @@ uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref) {
|
||||
|
||||
uint8_t Referee_PackAI(Referee_ForAI_t *ai, const Referee_t *ref) {
|
||||
ai->ref_status = ref->ref_status;
|
||||
memcpy(&(ai->robot_status), &(ref->robot_status), sizeof(ai->robot_status));
|
||||
memcpy(&(ai->game_status), &(ref->game_status), sizeof(ai->game_status));
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -701,6 +701,8 @@ typedef struct {
|
||||
|
||||
typedef struct {
|
||||
Referee_Status_t ref_status;
|
||||
Referee_RobotStatus_t robot_status;
|
||||
Referee_GameStatus_t game_status;
|
||||
} Referee_ForAI_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
@ -8,10 +8,10 @@ extern "C" {
|
||||
#include "device\device.h"
|
||||
//#include "referee.h"
|
||||
|
||||
#define SUPERCAP_CAN BSP_FDCAN_3
|
||||
#define SUPERCAP_CAN BSP_CAN_2
|
||||
|
||||
#define SUPERCAP_TX_ID 0x001 //C板发给超级电容的ID
|
||||
#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID
|
||||
#define SUPERCAP_TX_ID 0x30F //C板发给超级电容的ID
|
||||
#define SUPERCAP_RX_ID 0x300 //超级电容发给C板的ID
|
||||
|
||||
|
||||
//超级电容的状态标志位，超级电容运行或者保护的具体状态反馈
|
||||
|
||||
@ -64,6 +64,14 @@ fp32 vofa_send[8]; //vofa输出数据
|
||||
#define CHASSIS_ROTOR_OMEGA 0.001
|
||||
#define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
|
||||
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
|
||||
#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
|
||||
#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f /* 跟随角速度输出死区 */
|
||||
#define CHASSIS_FOLLOW_TRANS_DEADBAND 0.02f /* 跟随平移输入死区 */
|
||||
#define CHASSIS_FOLLOW_FREE_BAND_RAD 0.08f /* 跟随自由区，约4.6度 */
|
||||
#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
|
||||
#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f /* 跟随角速度输出死区 */
|
||||
#define CHASSIS_FOLLOW_DAMP_GAIN 0.08f /* 跟随阻尼系数 */
|
||||
#define CHASSIS_FOLLOW_VW_MAX 1.0f /* 跟随角速度上限 */
|
||||
|
||||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
|
||||
{
|
||||
@ -102,6 +110,41 @@ static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
|
||||
return wz_vary > 0.8f ? max : wz_vary;
|
||||
}
|
||||
|
||||
// static float Chassis_CalcFollowVw(Chassis_t *c)
|
||||
// {
|
||||
// const float follow_target = c->4.517f;
|
||||
// const float yaw_fb = c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle;
|
||||
// const float yaw_speed_fb = c->motorfeedback.gimbal_yaw_encoder.rotor_speed;
|
||||
// const float follow_err = CircleError(follow_target, yaw_fb, M_2PI);
|
||||
// const float abs_follow_err = fabsf(follow_err);
|
||||
|
||||
// if (abs_follow_err < CHASSIS_FOLLOW_LOCK_DEADBAND_RAD) {
|
||||
// PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
|
||||
// return 0.0f;
|
||||
// }
|
||||
|
||||
// /* 保留一段相对运动区，避免底盘与大YAW刚性同动 */
|
||||
// if (abs_follow_err < CHASSIS_FOLLOW_FREE_BAND_RAD) {
|
||||
// PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
|
||||
// return 0.0f;
|
||||
// }
|
||||
|
||||
// float follow_vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid,
|
||||
// follow_target,
|
||||
// yaw_fb,
|
||||
// 0.0f,
|
||||
// c->dt);
|
||||
|
||||
// /* 对大YAW速度做阻尼，降低同向拖动感 */
|
||||
// follow_vw -= CHASSIS_FOLLOW_DAMP_GAIN * yaw_speed_fb;
|
||||
// follow_vw = AbsClip(follow_vw, CHASSIS_FOLLOW_VW_MAX);
|
||||
|
||||
// if (fabsf(follow_vw) < CHASSIS_FOLLOW_VW_DEADBAND) {
|
||||
// follow_vw = 0.0f;
|
||||
// }
|
||||
// return follow_vw;
|
||||
// }
|
||||
|
||||
/*底盘初始化*/
|
||||
|
||||
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
|
||||
@ -130,7 +173,7 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
|
||||
}
|
||||
|
||||
// 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
|
||||
MotorOffset_t motor_offset = {{1.6467284 / M_PI * 180.0f, 5.77390385 / M_PI * 180.0f,
|
||||
MotorOffset_t motor_offset = {{1.6359905 / M_PI * 180.0f, 6.26170969 / M_PI * 180.0f,
|
||||
1.56696141 / M_PI * 180.0f, 5.77160311 / M_PI * 180.0f}}; // 右右右右
|
||||
|
||||
c->motoroffset = motor_offset;
|
||||
@ -405,9 +448,28 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
// 跟随云台模式
|
||||
c->move_vec.Vx =-c_cmd->Vy;
|
||||
c->move_vec.Vy =-c_cmd->Vx;
|
||||
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
|
||||
|
||||
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
|
||||
if (fabsf(c->move_vec.Vx) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vx = 0.0f;
|
||||
if (fabsf(c->move_vec.Vy) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vy = 0.0f;
|
||||
{
|
||||
const float follow_target = c->mech_zero_4310;
|
||||
const float follow_err = CircleError(follow_target,
|
||||
c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle,
|
||||
M_2PI);
|
||||
if (fabsf(follow_err) < CHASSIS_FOLLOW_LOCK_DEADBAND_RAD) {
|
||||
c->move_vec.Vw = 0.0f;
|
||||
PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
|
||||
} else {
|
||||
float follow_vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid,
|
||||
follow_target,
|
||||
c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle,
|
||||
0.0f,
|
||||
c->dt);
|
||||
if (fabsf(follow_vw) < CHASSIS_FOLLOW_VW_DEADBAND) {
|
||||
follow_vw = 0.0f;
|
||||
}
|
||||
c->move_vec.Vw = follow_vw;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
@ -488,10 +550,10 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
// }
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
/*电机输出设定和发送*/
|
||||
void Chassis_Setoutput(Chassis_t *c)
|
||||
{
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
|
||||
|
||||
@ -61,6 +61,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
|
||||
|
||||
/* 右摇杆控制云台 */
|
||||
ctx->output.gimbal.cmd.delta_yaw_6020 = -ctx->input.rc.joy_right.x * 2.0f;
|
||||
ctx->output.gimbal.cmd.delta_yaw_4310 = -ctx->input.rc.joy_right.x * 1.2f;
|
||||
ctx->output.gimbal.cmd.delta_pitch_4310 = -ctx->input.rc.joy_right.y * 1.5f;
|
||||
}
|
||||
|
||||
@ -71,19 +72,19 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
|
||||
} else {
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
|
||||
}
|
||||
/* 根据c拨杆控制射击 */
|
||||
/* C拨杆选择发弹模式：上单发，中多发，下连发 */
|
||||
switch (ctx->input.rc.sw[2]) {
|
||||
case CMD_SW_DOWN:
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE;
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
|
||||
break;
|
||||
case CMD_SW_UP:
|
||||
ctx->output.shoot.cmd.ready = SHOOT_MODE_SAFE;
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE;
|
||||
break;
|
||||
default:
|
||||
ctx->output.shoot.cmd.ready = false;
|
||||
ctx->output.shoot.cmd.firecmd = false;
|
||||
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
}
|
||||
/* 根据D拨杆控制射击 */
|
||||
@ -108,18 +109,22 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
|
||||
case CMD_SW_UP:
|
||||
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RC;
|
||||
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_REMOTE;
|
||||
ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_DAOHANG;
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_DAOHANG;
|
||||
ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RELAX;
|
||||
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI;
|
||||
ctx->output.shoot.cmd.control_mode = SHOOT_AI;
|
||||
break;
|
||||
default:
|
||||
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
||||
ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -101,7 +101,7 @@ Config_Param_t config = {
|
||||
/*这两个数据是云台6020的零点和机械限位*/
|
||||
.mech_zero = 1.31f,
|
||||
.travel = 1.4f,
|
||||
.mech_zero_4310=2.06f,
|
||||
.mech_zero_4310=4.51735544f,
|
||||
|
||||
|
||||
},
|
||||
@ -268,7 +268,7 @@ Config_Param_t config = {
|
||||
|
||||
.mech_zero = {
|
||||
.yaw_6020 = 1.31f,/*1.31*/
|
||||
.yaw_4310 = 2.06f, /*大yaw零点*/
|
||||
.yaw_4310 = 4.517f, /*大yaw零点*/
|
||||
.pitch_4310 = 0.93f,
|
||||
},
|
||||
.travel = {
|
||||
|
||||
@ -50,6 +50,27 @@ static double poly(double x) {
|
||||
return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984;
|
||||
}
|
||||
|
||||
static uint8_t g_scan_active = 0;
|
||||
static float g_scan_pitch_center = 0.0f;
|
||||
static float g_scan_pitch_dir = 1.0f;
|
||||
static float g_scan_yaw_dir = 1.0f;
|
||||
static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁存 */
|
||||
|
||||
#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.40f)
|
||||
#define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f)
|
||||
#define GIMBAL_SCAN_PITCH_SPEED (0.16f)
|
||||
#define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f)
|
||||
#define GIMBAL_SCAN_YAW_EDGE_MARGIN (0.16f)
|
||||
#define GIMBAL_SCAN_YAW_EDGE_HYST (0.22f)
|
||||
#define GIMBAL_SCAN_YAW_EDGE_RELEASE (0.28f)
|
||||
#define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.12f)
|
||||
#define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f)
|
||||
#define GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN (1.6f)
|
||||
#define GIMBAL_REMOTE_SMALL_YAW_GAIN (16.0f)
|
||||
#define GIMBAL_REMOTE_EDGE_ASSIST_BAND (0.20f)
|
||||
#define GIMBAL_YAW6020_INPUT_DEADBAND (0.0005f)
|
||||
#define GIMBAL_CONTROL_DT_MAX (0.02f)
|
||||
|
||||
|
||||
|
||||
/**
|
||||
@ -86,6 +107,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||
|
||||
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
|
||||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||
g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
||||
|
||||
g->mode = mode;
|
||||
return GIMBAL_OK;
|
||||
@ -112,6 +134,8 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
|
||||
}
|
||||
g->param = param;
|
||||
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 不输出*/
|
||||
g->last_wakeup = BSP_TIME_Get_us();
|
||||
g->dt = 0.0f;
|
||||
/*6020小yaw pid初始化*/
|
||||
PID_Init(&g->pid.yaw_6020_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.yaw_6020_motor_angle );
|
||||
PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega );
|
||||
@ -132,6 +156,8 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
|
||||
/*机械限位*/
|
||||
g->limit.yaw_6020.max = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020;
|
||||
g->limit.yaw_6020.min = g->param->mech_zero.yaw_6020 ;
|
||||
g->limit.yaw_4310.max = g->param->mech_zero.yaw_4310 + g->param->travel.yaw_4310;
|
||||
g->limit.yaw_4310.min = g->param->mech_zero.yaw_4310 ;
|
||||
g->limit.pitch_4310.max = g->param->mech_zero.pitch_4310 + g->param->travel.pitch_4310;
|
||||
g->limit.pitch_4310.min = g->param->mech_zero.pitch_4310 ;
|
||||
|
||||
@ -144,7 +170,7 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
|
||||
return GIMBAL_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
/**
|
||||
* \brief 通过CAN设备更新云台电机反馈信息
|
||||
*
|
||||
* \param gimbal 云台
|
||||
@ -152,7 +178,6 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
|
||||
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal)
|
||||
{
|
||||
if (gimbal == NULL)
|
||||
@ -194,7 +219,9 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||||
}
|
||||
gimbal->feedback.imu.gyro = imu->gyro;
|
||||
|
||||
gimbal->feedback.imu.eulr.yaw = imu->eulr.yaw;
|
||||
/* 先同步完整欧拉角，保证rol轴可用于90度装配下的pitch控制 */
|
||||
gimbal->feedback.imu.eulr = imu->eulr;
|
||||
/* 保留pit映射，兼容现有使用pit坐标的逻辑 */
|
||||
gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换
|
||||
|
||||
gimbal->feedback.imu.quat = imu->quat;
|
||||
@ -245,33 +272,144 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||||
return GIMBAL_ERR; // 参数错误
|
||||
}
|
||||
|
||||
g->dt = (BSP_TIME_Get_us() - g->last_wakeup) / 1000000.0f;
|
||||
g->last_wakeup = BSP_TIME_Get_us();
|
||||
{
|
||||
uint64_t now_us = BSP_TIME_Get_us();
|
||||
g->dt = (now_us - g->last_wakeup) / 1000000.0f;
|
||||
g->last_wakeup = now_us;
|
||||
if (g->dt < 0.0f || g->dt > GIMBAL_CONTROL_DT_MAX) {
|
||||
g->dt = 0.002f;
|
||||
}
|
||||
}
|
||||
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
if (g_cmd->ctrl_mode == GIMBAL_MODE_SCAN) {
|
||||
float yaw_speed_scale = 1.0f;
|
||||
float big_yaw_speed = 0.0f;
|
||||
float pitch_speed_scale = 1.0f;
|
||||
float yaw_abs = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
|
||||
float pitch_abs = g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle;
|
||||
|
||||
if (!g_scan_active) {
|
||||
float yaw_mid = 0.5f * (g->limit.yaw_6020.max + g->limit.yaw_6020.min);
|
||||
g_scan_active = 1;
|
||||
g_scan_pitch_center = g->feedback.imu.eulr.rol;
|
||||
g_scan_pitch_dir = 1.0f;
|
||||
g_scan_yaw_dir = (yaw_abs >= yaw_mid) ? -1.0f : 1.0f;
|
||||
g_scan_yaw_edge_latch = 0;
|
||||
g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
||||
}
|
||||
|
||||
if (GIMBAL_SCAN_BIG_YAW_PERIOD_SEC > 0.0f) {
|
||||
/* 连续旋转：按周期计算角速度，5秒转一圈即 2PI/5 */
|
||||
big_yaw_speed = M_2PI / GIMBAL_SCAN_BIG_YAW_PERIOD_SEC;
|
||||
}
|
||||
|
||||
if (g_scan_yaw_edge_latch == 1) {
|
||||
g_scan_yaw_dir = -1.0f;
|
||||
if (yaw_abs < g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_RELEASE) {
|
||||
g_scan_yaw_edge_latch = 0;
|
||||
}
|
||||
} else if (g_scan_yaw_edge_latch == -1) {
|
||||
g_scan_yaw_dir = 1.0f;
|
||||
if (yaw_abs > g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_RELEASE) {
|
||||
g_scan_yaw_edge_latch = 0;
|
||||
}
|
||||
} else if (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) {
|
||||
g_scan_yaw_dir = -1.0f;
|
||||
g_scan_yaw_edge_latch = 1;
|
||||
} else if (yaw_abs <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN) {
|
||||
g_scan_yaw_dir = 1.0f;
|
||||
g_scan_yaw_edge_latch = -1;
|
||||
} else if (g_scan_yaw_dir > 0.0f && yaw_abs > g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_HYST) {
|
||||
g_scan_yaw_dir = -1.0f;
|
||||
} else if (g_scan_yaw_dir < 0.0f && yaw_abs < g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_HYST) {
|
||||
g_scan_yaw_dir = 1.0f;
|
||||
}
|
||||
|
||||
if (pitch_abs >= g->limit.pitch_4310.max - GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
|
||||
g_scan_pitch_dir = -1.0f;
|
||||
} else if (pitch_abs <= g->limit.pitch_4310.min + GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
|
||||
g_scan_pitch_dir = 1.0f;
|
||||
}
|
||||
|
||||
{
|
||||
float yaw_dist_to_edge_max = g->limit.yaw_6020.max - yaw_abs;
|
||||
float yaw_dist_to_edge_min = yaw_abs - g->limit.yaw_6020.min;
|
||||
float yaw_min_dist = (yaw_dist_to_edge_max < yaw_dist_to_edge_min) ? yaw_dist_to_edge_max : yaw_dist_to_edge_min;
|
||||
if (yaw_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) {
|
||||
if (yaw_min_dist < 0.0f) yaw_min_dist = 0.0f;
|
||||
yaw_speed_scale = (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
float pitch_dist_to_edge_max = g->limit.pitch_4310.max - pitch_abs;
|
||||
float pitch_dist_to_edge_min = pitch_abs - g->limit.pitch_4310.min;
|
||||
float pitch_min_dist = (pitch_dist_to_edge_max < pitch_dist_to_edge_min) ? pitch_dist_to_edge_max : pitch_dist_to_edge_min;
|
||||
if (pitch_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) {
|
||||
if (pitch_min_dist < 0.0f) pitch_min_dist = 0.0f;
|
||||
pitch_speed_scale = 0.25f + 0.75f * (pitch_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
|
||||
}
|
||||
}
|
||||
|
||||
if (g->feedback.imu.eulr.rol >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) {
|
||||
g_scan_pitch_dir = -1.0f;
|
||||
} else if (g->feedback.imu.eulr.rol <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) {
|
||||
g_scan_pitch_dir = 1.0f;
|
||||
}
|
||||
|
||||
g_cmd->delta_yaw_6020 = g_scan_yaw_dir * GIMBAL_SCAN_SMALL_YAW_SPEED * yaw_speed_scale;
|
||||
g_cmd->delta_pitch_4310 = g_scan_pitch_dir * GIMBAL_SCAN_PITCH_SPEED * pitch_speed_scale;
|
||||
if (g->param->travel.yaw_4310 > 0.0f) {
|
||||
g->setpoint.yaw_4310 += big_yaw_speed * g->dt;
|
||||
if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max;
|
||||
if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min;
|
||||
} else {
|
||||
CircleAdd(&(g->setpoint.yaw_4310), big_yaw_speed * g->dt, M_2PI);
|
||||
}
|
||||
g_cmd->yaw_vel = 0.0f;
|
||||
g_cmd->yaw_accl = 0.0f;
|
||||
g_cmd->pit_vel = 0.0f;
|
||||
g_cmd->pit_accl = 0.0f;
|
||||
} else {
|
||||
g_scan_active = 0;
|
||||
g_scan_yaw_dir = 1.0f;
|
||||
g_scan_yaw_edge_latch = 0;
|
||||
}
|
||||
|
||||
/*处理小yaw轴控制命令*/
|
||||
float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
|
||||
|
||||
if(g->param->travel.yaw_6020 > 0) // 有限位才处理
|
||||
{
|
||||
/* 计算当前电机角度与IMU角度的偏差 */
|
||||
/*float*/
|
||||
motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw;
|
||||
/* 处理跨越±π的情况 */
|
||||
if (motor_imu_offset_6020 > M_PI) motor_imu_offset_6020 -= M_2PI;
|
||||
if (motor_imu_offset_6020 < -M_PI) motor_imu_offset_6020 += M_2PI;
|
||||
|
||||
/*计算限位距离*/
|
||||
const float delta_max_6020 = CircleError(g->limit.yaw_6020.max,
|
||||
(g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);
|
||||
const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
|
||||
(g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);
|
||||
|
||||
if(delta_yaw_6020 > delta_max_6020) delta_yaw_6020 = delta_max_6020;
|
||||
if(delta_yaw_6020 < delta_min_6020) delta_yaw_6020 = delta_min_6020;
|
||||
float yaw_6020_gain = 10.0f;
|
||||
if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) {
|
||||
yaw_6020_gain = GIMBAL_REMOTE_SMALL_YAW_GAIN;
|
||||
}
|
||||
float delta_yaw_6020 = yaw_6020_gain * g_cmd->delta_yaw_6020 * g->dt;
|
||||
float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
|
||||
float yaw_abs_target = yaw_abs_now;
|
||||
|
||||
/* 摇杆无输入时抑制微小噪声，避免setpoint慢漂 */
|
||||
if (fabsf(delta_yaw_6020) < GIMBAL_YAW6020_INPUT_DEADBAND) {
|
||||
delta_yaw_6020 = 0.0f;
|
||||
}
|
||||
|
||||
if (g->param->travel.yaw_6020 > 0.0f) {
|
||||
/* 边界方向判定：到边界时只允许朝边界内侧运动 */
|
||||
if (yaw_abs_now >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 > 0.0f) {
|
||||
delta_yaw_6020 = 0.0f;
|
||||
}
|
||||
if (yaw_abs_now <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 < 0.0f) {
|
||||
delta_yaw_6020 = 0.0f;
|
||||
}
|
||||
|
||||
yaw_abs_target = yaw_abs_now + delta_yaw_6020;
|
||||
if (yaw_abs_target > g->limit.yaw_6020.max) yaw_abs_target = g->limit.yaw_6020.max;
|
||||
if (yaw_abs_target < g->limit.yaw_6020.min) yaw_abs_target = g->limit.yaw_6020.min;
|
||||
|
||||
/* 直接用允许的电机角增量映射到欧拉角目标，避免CircleAdd累积导致回弹 */
|
||||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw + (yaw_abs_target - yaw_abs_now);
|
||||
} else {
|
||||
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI);
|
||||
}
|
||||
|
||||
// g->setpoint.eulr.yaw = g->setpoint.NUC_Yaw;
|
||||
|
||||
@ -279,9 +417,30 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
|
||||
/*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
|
||||
跟随小YAW向相同方向运动，将遥控数据进行分配*/
|
||||
|
||||
/*小YAW中点*/
|
||||
float small_yaw_center_offset = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020* 0.5f ;
|
||||
g->setpoint.yaw_4310=small_yaw_center_offset;
|
||||
if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) {
|
||||
float delta_yaw_4310 = -GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN * g_cmd->delta_yaw_4310 * g->dt;
|
||||
if (g->param->travel.yaw_6020 > 0.0f) {
|
||||
float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
|
||||
float dist_to_max = g->limit.yaw_6020.max - yaw_abs_now;
|
||||
float dist_to_min = yaw_abs_now - g->limit.yaw_6020.min;
|
||||
float edge_assist = 1.0f;
|
||||
if (g_cmd->delta_yaw_6020 > 0.0f && dist_to_max < GIMBAL_REMOTE_EDGE_ASSIST_BAND) {
|
||||
edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_max) / GIMBAL_REMOTE_EDGE_ASSIST_BAND;
|
||||
} else if (g_cmd->delta_yaw_6020 < 0.0f && dist_to_min < GIMBAL_REMOTE_EDGE_ASSIST_BAND) {
|
||||
edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_min) / GIMBAL_REMOTE_EDGE_ASSIST_BAND;
|
||||
}
|
||||
if (edge_assist < 1.0f) edge_assist = 1.0f;
|
||||
if (edge_assist > 2.0f) edge_assist = 2.0f;
|
||||
delta_yaw_4310 *= edge_assist;
|
||||
}
|
||||
if (g->param->travel.yaw_4310 > 0.0f) {
|
||||
g->setpoint.yaw_4310 += delta_yaw_4310;
|
||||
if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max;
|
||||
if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min;
|
||||
} else {
|
||||
CircleAdd(&(g->setpoint.yaw_4310), delta_yaw_4310, M_2PI);
|
||||
}
|
||||
}
|
||||
|
||||
/*处理大pitch控制命令*/
|
||||
float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt;
|
||||
@ -319,6 +478,11 @@ switch (g_cmd->ctrl_mode) {
|
||||
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_SCAN:
|
||||
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
|
||||
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_AI:
|
||||
g->setpoint.eulr.yaw = g_cmd->set_yaw;
|
||||
g->setpoint.eulr.pit = g_cmd->set_pitch;
|
||||
@ -354,7 +518,7 @@ switch (g->mode)
|
||||
|
||||
|
||||
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
||||
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
|
||||
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt);
|
||||
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
||||
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
|
||||
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
||||
|
||||
@ -116,6 +116,7 @@ extern "C"
|
||||
{
|
||||
GIMBAL_MODE_REMOTE,
|
||||
GIMBAL_MODE_AI,
|
||||
GIMBAL_MODE_SCAN,
|
||||
}GIMBAL_Ctrl_mode_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
@ -17,7 +17,9 @@
|
||||
/* USER STRUCT BEGIN */
|
||||
PackageAI_t ai;
|
||||
PackageMCU_t mcu;
|
||||
PackageReferee_t ref_pkg;
|
||||
AI_result_t ai_result;
|
||||
Referee_ForAI_t ref_for_ai;
|
||||
AHRS_Quaternion_t quat;
|
||||
Gimbal_feedback_t gimbal_motor;
|
||||
PackageMCU_t shoot_mcu_package; /* 新增的 ai 数据包 主要是给自瞄发送射击数量*/
|
||||
@ -45,10 +47,15 @@ void Task_ai(void *argument) {
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.quat, &quat, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.feedback, &gimbal_motor, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_mcu_package, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.referee.ai, &ref_for_ai, NULL, 0);
|
||||
MCU_Send(&mcu,&gimbal_motor,&quat);
|
||||
mcu.data.bullet_count=shoot_mcu_package.data.bullet_count; /* 从 shoot 任务获取射击数量并放入 mcu 数据包中 */
|
||||
MCU_StartSend(&mcu);
|
||||
|
||||
if (REF_Send(&ref_pkg, &ref_for_ai.robot_status, &ref_for_ai.game_status) == DEVICE_OK) {
|
||||
REF_StartSend(&ref_pkg);
|
||||
}
|
||||
|
||||
AI_StartReceiving(&ai);
|
||||
AI_Get_NUC(&ai,&ai_result);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
|
||||
|
||||
@ -9,6 +9,8 @@
|
||||
#include "module/chassis.h"
|
||||
#include "module/config.h"
|
||||
#include "device/ai.h"
|
||||
#include "device/supercap.h"
|
||||
#include "component/limiter.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -19,6 +21,8 @@
|
||||
Chassis_t chassis;
|
||||
Chassis_CMD_t cmd_chassis;
|
||||
AI_result_t c_cmd_ai_result; /* 新增的 ai 结果变量 主要是给底盘接收自瞄相关的命令*/
|
||||
float temp_for_cap[7];//cap-add//supercap-add
|
||||
float P3508[4],P6020[4],I6020[4],S6020[4],I3508[4],S3508[4],power_limit_3508=100;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -66,6 +70,54 @@ osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
|
||||
Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
|
||||
Chassis_Control(&chassis, &safe_cmd,tick);
|
||||
}
|
||||
|
||||
// float P3=0,P6=0;
|
||||
// for(int i=0;i<4;i++)
|
||||
// {
|
||||
// S6020[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&chassis.param->motor_6020_param[i])->motor));
|
||||
// I6020[i] = MOTOR_GetTorqueCurrent(&(MOTOR_RM_GetMotor(&chassis.param->motor_6020_param[i])->motor));
|
||||
// S3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&chassis.param->motor_3508_param[i])->motor));
|
||||
// I3508[i] = MOTOR_GetTorqueCurrent(&(MOTOR_RM_GetMotor(&chassis.param->motor_3508_param[i])->motor));
|
||||
// P3508[i]=fabsf(S3508[i]*I3508[i]);
|
||||
// P6020[i]=fabsf(S6020[i]*I6020[i]);
|
||||
// P3+=P3508[i];
|
||||
// P6+=P6020[i];
|
||||
// }
|
||||
// temp_for_cap[0] = P6*0.000012; // 计算6020总功率 P6
|
||||
// temp_for_cap[1] = P3*0.0000048;
|
||||
// float P_3508 = 0; // 计算3508总功率 P3
|
||||
// for(int i=0;i<4;i++)
|
||||
// {
|
||||
// P_3508+=chassis.out.rotor3508_out[i]*S3508[i]*0.0000048;
|
||||
// }
|
||||
|
||||
|
||||
// if(CAN_SuperCapRXData.SuperCapReady == 1)
|
||||
// {
|
||||
// if(CAN_SuperCapRXData.SuperCapEnergy<=10)
|
||||
// power_limit_3508 = 100;
|
||||
// else
|
||||
// power_limit_3508 = 320;
|
||||
|
||||
// if(CAN_SuperCapRXData.ChassisPower>power_limit_3508&&power_limit_3508>=0)
|
||||
// {
|
||||
// for(int i=0;i<4;i++)
|
||||
// {
|
||||
// chassis.out.rotor3508_out[i]*=(power_limit_3508)/
|
||||
// CAN_SuperCapRXData.ChassisPower>power_limit_3508; // 如果总功率超过限制，按比例降低3508的输出
|
||||
// }
|
||||
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// //启动老方案进行限制
|
||||
// PowerLimit_ChassicOutput(power_limit_3508,chassis.out.rotor3508_out,
|
||||
// chassis.motorfeedback.rotor_rpm3508, 4); // 根据功率限制调整3508输出
|
||||
|
||||
// }
|
||||
|
||||
|
||||
Chassis_Setoutput(&chassis);
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
|
||||
@ -64,7 +64,7 @@
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
frequency: 10.0
|
||||
function: Task_super_cap
|
||||
name: super_cap
|
||||
stack: 256
|
||||
|
||||
@ -64,35 +64,26 @@ osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL,
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
|
||||
|
||||
if(ai_gimbal_result_cmd.mode==0){
|
||||
cmd_gimbal.set_pitch =gimbal_imu.eulr.pit;
|
||||
cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;
|
||||
|
||||
if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
|
||||
if (ai_gimbal_result_cmd.mode == 0) {
|
||||
cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN;
|
||||
} else {
|
||||
cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd_gimbal.ctrl_mode = GIMBAL_MODE_AI;
|
||||
cmd_gimbal.set_pitch = ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw = ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
|
||||
cmd_gimbal.yaw_vel = ai_gimbal_result_cmd.gimbal_t.vel.yaw;
|
||||
cmd_gimbal.yaw_accl = ai_gimbal_result_cmd.gimbal_t.accl.yaw;
|
||||
cmd_gimbal.pit_vel = ai_gimbal_result_cmd.gimbal_t.vel.pit;
|
||||
cmd_gimbal.pit_accl = ai_gimbal_result_cmd.gimbal_t.accl.pit;
|
||||
}
|
||||
if(ai_gimbal_result_cmd.mode==1)
|
||||
{
|
||||
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw=ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
|
||||
cmd_gimbal.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
|
||||
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
|
||||
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
|
||||
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
|
||||
|
||||
}
|
||||
if(ai_gimbal_result_cmd.mode==2){
|
||||
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw=ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
|
||||
cmd_gimbal.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
|
||||
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
|
||||
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
|
||||
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
|
||||
|
||||
}
|
||||
|
||||
|
||||
Gimbal_UpdateFeedback(&gimbal);
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);
|
||||
/* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.yaw6020, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0);
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.ai.feedback);
|
||||
|
||||
@ -21,13 +21,10 @@ Referee_ForCap_t for_cap;
|
||||
Referee_ForAI_t for_ai;
|
||||
Referee_ForChassis_t for_chassis;
|
||||
Referee_ForShoot_t for_shoot;
|
||||
int hhh;
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* USER PRIVATE CODE BEGIN */
|
||||
|
||||
/* USER PRIVATE CODE END */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_referee(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
@ -50,7 +47,7 @@ void Task_referee(void *argument) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
Referee_StartReceiving(&ref);
|
||||
if(ref.ref_status==0){hhh++;}
|
||||
|
||||
if (osThreadFlagsWait(SIGNAL_REFEREE_RAW_REDY, osFlagsWaitAll, 10) !=
|
||||
SIGNAL_REFEREE_RAW_REDY) {
|
||||
if (osKernelGetTickCount() - last_online_tick > 3000)
|
||||
|
||||
@ -41,6 +41,10 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
|
||||
shoot_ai_mcu_package.data.bullet_count = 0;
|
||||
static bool last_fire_state = false;
|
||||
bool current_fire_state = false; // 当前是否需要发射
|
||||
static uint32_t ai_last_fire_tick = 0;
|
||||
static const uint32_t ai_single_interval_ms = 220;
|
||||
static const uint32_t ai_burst_interval_ms = 110;
|
||||
static const uint32_t ai_continue_interval_ms = 0;
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
@ -56,20 +60,39 @@ if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
||||
}
|
||||
else if(shoot_cmd.control_mode==SHOOT_AI)
|
||||
{
|
||||
// shoot_cmd.mode = SHOOT_MODE_SINGLE;
|
||||
// if(shoot_ai_result_cmd.mode==0 || shoot_ai_result_cmd.mode==1)
|
||||
// {
|
||||
// shoot_cmd.firecmd=false;
|
||||
// shoot_cmd.ready=true;
|
||||
uint32_t now_ms = osKernelGetTickCount();
|
||||
uint32_t interval_ms = ai_single_interval_ms;
|
||||
|
||||
// }
|
||||
// else if(shoot_ai_result_cmd.mode==2)
|
||||
// {
|
||||
// shoot_cmd.firecmd=true;
|
||||
// shoot_cmd.ready=true;
|
||||
// current_fire_state = true;
|
||||
// }
|
||||
current_fire_state = shoot_cmd.firecmd;
|
||||
shoot_cmd.ready = true;
|
||||
|
||||
if (shoot_ai_result_cmd.mode == 2) {
|
||||
switch (shoot_cmd.mode) {
|
||||
case SHOOT_MODE_CONTINUE:
|
||||
interval_ms = ai_continue_interval_ms;
|
||||
break;
|
||||
case SHOOT_MODE_BURST:
|
||||
interval_ms = ai_burst_interval_ms;
|
||||
break;
|
||||
case SHOOT_MODE_SINGLE:
|
||||
default:
|
||||
interval_ms = ai_single_interval_ms;
|
||||
break;
|
||||
}
|
||||
|
||||
if (interval_ms == 0) {
|
||||
shoot_cmd.firecmd = true;
|
||||
} else if ((now_ms - ai_last_fire_tick) >= interval_ms) {
|
||||
shoot_cmd.firecmd = true;
|
||||
ai_last_fire_tick = now_ms;
|
||||
} else {
|
||||
shoot_cmd.firecmd = false;
|
||||
}
|
||||
} else {
|
||||
shoot_cmd.firecmd = false;
|
||||
ai_last_fire_tick = now_ms;
|
||||
}
|
||||
|
||||
current_fire_state = shoot_cmd.firecmd;
|
||||
}
|
||||
|
||||
if(current_fire_state == true && last_fire_state == false)
|
||||
|
||||
@ -23,9 +23,6 @@ Referee_CapUI_t cap_ui;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* USER PRIVATE CODE BEGIN */
|
||||
|
||||
/* USER PRIVATE CODE END */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_super_cap(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
@ -17,7 +17,7 @@ const osThreadAttr_t attr_atti_esti = {
|
||||
const osThreadAttr_t attr_rc = {
|
||||
.name = "rc",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 512 * 4,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_chassis = {
|
||||
.name = "chassis",
|
||||
@ -27,7 +27,7 @@ const osThreadAttr_t attr_chassis = {
|
||||
const osThreadAttr_t attr_cmd = {
|
||||
.name = "cmd",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 512 * 4,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_gimbal = {
|
||||
.name = "gimbal",
|
||||
|
||||
@ -22,7 +22,7 @@ extern "C" {
|
||||
#define AI_FREQ (250.0)
|
||||
#define REFEREE_FREQ (500.0)
|
||||
#define TASK9_FREQ (500.0)
|
||||
#define SUPER_CAP_FREQ (500.0)
|
||||
#define SUPER_CAP_FREQ (10.0)
|
||||
|
||||
/* 任务初始化延时ms */
|
||||
#define TASK_INIT_DELAY (100u)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user