From 37c23ddb795f7c1a51c9dc8852d4d877c3ba5351 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Fri, 9 Dec 2022 18:25:35 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E7=82=B9=E5=87=BBpi?= =?UTF-8?q?d=E8=AE=A1=E7=AE=97=E6=B5=81=E7=A8=8B=EF=BC=8C=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=E4=BA=86bsp=E6=9C=AA=E5=88=9D=E5=A7=8B=E5=8C=96?= =?UTF-8?q?=E7=9A=84=E5=BC=82=E5=B8=B8=EF=BC=8C=E4=BF=AE=E5=A4=8D=E4=BA=86?= =?UTF-8?q?=E6=95=B0=E4=B8=AA=E6=9C=AA=E5=AE=9A=E4=B9=89=E5=92=8C=E9=9A=90?= =?UTF-8?q?=E5=BC=8F=E5=A3=B0=E6=98=8E=E7=9A=84=E8=AD=A6=E5=91=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 2 +- .vscode/settings.json | 1 + HAL_N_Middlewares/Src/freertos.c | 2 +- HAL_N_Middlewares/Src/main.c | 2 -- application/chassis/chassis.c | 41 ++++++++++++------------ application/cmd/robot_cmd.c | 6 ++-- application/gimbal/gimbal.c | 32 +++++++++++-------- application/robot.c | 2 ++ application/robot_def.h | 8 ++++- application/shoot/shoot.c | 53 ++++++++++++++++++-------------- modules/algorithm/controller.c | 3 +- modules/daemon/daemon.c | 1 + modules/general_def.h | 2 ++ modules/imu/ins_task.c | 1 + modules/motor/dji_motor.c | 11 ++++--- modules/remote/remote_control.c | 12 ++++---- 16 files changed, 104 insertions(+), 75 deletions(-) diff --git a/.gitignore b/.gitignore index 6f37ac8..bbb2d53 100644 --- a/.gitignore +++ b/.gitignore @@ -51,4 +51,4 @@ build ./idea .vscode/.cortex-debug.peripherals.state.json .vscode/.cortex-debug.registers.state.json -*.jdebug +*.jdebug* diff --git a/.vscode/settings.json b/.vscode/settings.json index 3a53a10..d01c3b5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -39,6 +39,7 @@ "general_def.h": "c", "super_cap.h": "c", "motor_def.h": "c", + "quaternionekf.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index 04d0536..391e140 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -186,7 +186,7 @@ void StartDAEMONTASK(void const * argument) { while (1) { - //500Hz + //100Hz DaemonTask(); osDelay(10); } diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index f599d14..b89d655 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -33,7 +33,6 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "robot.h" -#include "message_center.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -114,7 +113,6 @@ int main(void) RobotInit(); - /* USER CODE END 2 */ /* Call init function for freertos objects (in freertos.c) */ diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 3fcf815..2141ff0 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -59,17 +59,19 @@ void ChassisInit() { // 四个轮子的参数一样,改tx_id和反转标志位即可 Motor_Init_Config_s chassis_motor_config = { - .can_init_config.can_handle=&hcan1, + .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp=10, - .Ki=0, - .Kd=0, + .Kp = 10, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, .current_PID = { - .Kp=10, - .Ki=0, - .Kd=0, + .Kp = 10, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, }, .controller_setting_init_config = { @@ -78,26 +80,27 @@ void ChassisInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, }, - .motor_type = M3508}; - - chassis_motor_config.can_init_config.tx_id=1; - chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + .motor_type = M3508, + }; + + chassis_motor_config.can_init_config.tx_id = 1; + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id=2, - chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + chassis_motor_config.can_init_config.tx_id = 2, + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rf = DJIMotorInit(&chassis_motor_config); - - chassis_motor_config.can_init_config.tx_id=3, - chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + + chassis_motor_config.can_init_config.tx_id = 3, + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id=4, - chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + chassis_motor_config.can_init_config.tx_id = 4, + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); referee_data = RefereeInit(&huart6); - + SuperCap_Init_Config_s cap_conf = { .can_config = { .can_handle = &hcan2, diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index e807a35..ba965fe 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -99,8 +99,8 @@ static void RemoteControlSet() if (switch_is_down(rc_data[TEMP].rc.s[1]) || vision_recv_data->target_state == NO_TARGET) { // 按照摇杆的输出大小进行角度增量,增益系数需调整 gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.joystick[2]; - gimbal_cmd_send.pitch += 0.0025f * (float)rc_data[TEMP].rc.joystick[3]; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.joystick[3]; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; } // 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整 @@ -145,7 +145,7 @@ static void EmergencyHandler() robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式 gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; - shoot_cmd_send.load_mode = SHOOT_STOP; + shoot_cmd_send.shoot_mode = SHOOT_OFF; return; } // if(rc_data[TEMP].rc.joystick[4]<-300 && 各个模块正常) diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index f8fb668..957dc2d 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -16,24 +16,27 @@ static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息 void GimbalInit() { - Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 + // Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { .can_init_config = { .can_handle = &hcan1, - .tx_id = 2, + .tx_id = 1, }, .controller_param_init_config = { .angle_PID = { - .Kd = 1, + .Kp = 10, .Ki = 0, .Kd = 0, + .MaxOut = 2000, + .DeadBand=0.3, }, .speed_PID = { - .Kd = 1, + .Kp = 10, .Ki = 0, .Kd = 0, + .MaxOut = 2000, }, .other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针 @@ -44,25 +47,28 @@ void GimbalInit() .speed_feedback_source = MOTOR_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, + .reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020}; // PITCH Motor_Init_Config_s pitch_config = { .can_init_config = { .can_handle = &hcan1, - .tx_id = 1, + .tx_id = 3, }, .controller_param_init_config = { .angle_PID = { - .Kd = 10, - .Ki = 1, - .Kd = 2, - }, - .speed_PID = { - .Kd = 1, + .Kp = 10, .Ki = 0, .Kd = 0, + .MaxOut = 2000, + .DeadBand=0.3, + }, + .speed_PID = { + .Kp = 10, + .Ki = 0, + .Kd = 0, + .MaxOut = 2000, }, .other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch, // 还需要增加角速度额外反馈指针 @@ -73,7 +79,7 @@ void GimbalInit() .speed_feedback_source = MOTOR_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, + .reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, }; diff --git a/application/robot.c b/application/robot.c index a29b6b5..9a11c9a 100644 --- a/application/robot.c +++ b/application/robot.c @@ -1,3 +1,4 @@ +#include "bsp_init.h" #include "robot.h" #include "robot_def.h" @@ -12,6 +13,7 @@ void RobotInit() { + BSPInit(); #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) ChassisInit(); #endif diff --git a/application/robot_def.h b/application/robot_def.h index 3d30cc5..17b23f4 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -88,8 +88,14 @@ typedef enum GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 } gimbal_mode_e; + // 发射模式设置 typedef enum +{ + SHOOT_ON, + SHOOT_OFF, +}shoot_mode_e; +typedef enum { FRICTION_OFF, // 摩擦轮关闭 FRICTION_ON, // 摩擦轮开启 @@ -103,7 +109,6 @@ typedef enum typedef enum { - SHOOT_STOP, // 停止整个发射模块,后续可能隔离出来 LOAD_STOP, // 停止发射 LOAD_REVERSE, // 反转 LOAD_1_BULLET, // 单发 @@ -150,6 +155,7 @@ typedef struct // cmd发布的发射控制数据,由shoot订阅 typedef struct { + shoot_mode_e shoot_mode; loader_mode_e load_mode; lid_mode_e lid_mode; friction_mode_e friction_mode; diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e84ac5a..7aa4f10 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -29,14 +29,16 @@ void ShootInit() }, .controller_param_init_config = { .speed_PID = { - .Kp=10, - .Ki=0, - .Kd=0, + .Kp = 10, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, .current_PID = { - .Kp=10, - .Ki=0, - .Kd=0, + .Kp = 10, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, }, .controller_setting_init_config = { @@ -56,14 +58,16 @@ void ShootInit() }, .controller_param_init_config = { .speed_PID = { - .Kp=1, - .Ki=0, - .Kd=0, + .Kp = 1, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, .current_PID = { - .Kp=1, - .Ki=0, - .Kd=0, + .Kp = 1, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, }, .controller_setting_init_config = { @@ -83,19 +87,22 @@ void ShootInit() .controller_param_init_config = { .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kd = 10, + .Kp = 10, .Ki = 1, .Kd = 2, + .MaxOut = 200, }, .speed_PID = { - .Kp=1, - .Ki=0, - .Kd=0, + .Kp = 1, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, .current_PID = { - .Kp=1, - .Ki=0, - .Kd=0, + .Kp = 1, + .Ki = 0, + .Kd = 0, + .MaxOut = 200, }, }, .controller_setting_init_config = { @@ -121,7 +128,7 @@ void ShootTask() SubGetMessage(shoot_sub, &shoot_cmd_recv); // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机 - if (shoot_cmd_recv.load_mode == SHOOT_STOP) + if (shoot_cmd_recv.shoot_mode == SHOOT_OFF) { DJIMotorStop(friction_l); DJIMotorStop(friction_r); @@ -175,15 +182,15 @@ void ShootTask() { case SMALL_AMU_15: DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_r, 0); break; case SMALL_AMU_18: DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_r, 0); break; case SMALL_AMU_30: DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_r, 0); break; default: break; diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c index e982dd2..836a88b 100644 --- a/modules/algorithm/controller.c +++ b/modules/algorithm/controller.c @@ -29,10 +29,11 @@ static void f_PID_ErrorHandle(PIDInstance *pid); // 堵转保护 */ void PID_Init(PIDInstance *pid, PID_Init_config_s *config) { + memset(pid, 0, sizeof(PIDInstance)); // utilize the quality of struct that its memeory is continuous memcpy(pid, config, sizeof(PID_Init_config_s)); // set rest of memory to 0 - memset(&pid->Measure, 0, sizeof(PIDInstance) - sizeof(PID_Init_config_s)); + } /** diff --git a/modules/daemon/daemon.c b/modules/daemon/daemon.c index 2733c91..6520c8b 100644 --- a/modules/daemon/daemon.c +++ b/modules/daemon/daemon.c @@ -1,5 +1,6 @@ #include "daemon.h" #include "bsp_dwt.h" // 后续通过定时器来计时? +#include "stdlib.h" static DaemonInstance *daemon_instances[DAEMON_MX_CNT] = {NULL}; static uint8_t idx; diff --git a/modules/general_def.h b/modules/general_def.h index b8cbf11..2656571 100644 --- a/modules/general_def.h +++ b/modules/general_def.h @@ -1,7 +1,9 @@ #ifndef GENERAL_DEF_H #define GENERAL_DEF_H +#ifndef PI #define PI 3.1415926535f +#endif // !PI #define PI2 (PI * 2.0f) // 2 pi #define RAD_2_ANGLE (180.0f / PI) diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index a75c6ff..d70f575 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -68,6 +68,7 @@ attitude_t *INS_Init(void) // noise of accel is relatively big and of high freq,thus lpf is used INS.AccelLPF = 0.0085; + return (attitude_t*)&INS.Roll; } /* 注意以1kHz的频率运行此任务 */ diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index f0d96c3..ab8617b 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -223,7 +223,7 @@ void DJIMotorControl() static Motor_Control_Setting_s *motor_setting; static Motor_Controller_s *motor_controller; static DJI_Motor_Measure_s *motor_measure; - static float pid_measure; + static float pid_measure,pid_ref; // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 for (size_t i = 0; i < idx; i++) { @@ -233,6 +233,7 @@ void DJIMotorControl() motor_setting = &motor->motor_settings; motor_controller = &motor->motor_controller; motor_measure = &motor->motor_measure; + pid_ref=motor_controller->pid_ref; //保存设定值,防止motor_controller->pid_ref在计算过程中被修改 // pid_ref会顺次通过被启用的闭环充当数据的载体 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 @@ -243,7 +244,7 @@ void DJIMotorControl() else // MOTOR_FEED pid_measure = motor_measure->total_angle; // 对total angle闭环,防止在边界处出现突跃 // 更新pid_ref进入下一个环 - motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref); + pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, pid_ref); } // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环 @@ -254,17 +255,17 @@ void DJIMotorControl() else // MOTOR_FEED pid_measure = motor_measure->speed_angle_per_sec; // 更新pid_ref进入下一个环 - motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref); + pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, pid_ref); } // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 if (motor_setting->close_loop_type & CURRENT_LOOP) { - motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref); + pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, pid_ref); } // 获取最终输出 - set = (int16_t)motor_controller->pid_ref; + set = (int16_t)pid_ref; if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; // 设置反转 diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index cd94c54..9382476 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -19,11 +19,11 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) { memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据 // 摇杆,直接解算时减去偏置 - rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0 - rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1 - rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2 - rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3 - rc_ctrl[TEMP].rc.joystick[4] = (sbus_buf[16] | (sbus_buf[17] << 8)) - RC_CH_VALUE_OFFSET; // 左侧拨轮 + rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 0 + rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 1 + rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2 + rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 3 + rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET; // 左侧拨轮 // 开关,0左1右 rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right @@ -81,5 +81,5 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle) conf.usart_handle = rc_usart_handle; conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE; rc_usart_instance = USARTRegister(&conf); - return &rc_ctrl; + return (RC_ctrl_t*)&rc_ctrl; } \ No newline at end of file