From 817f3a771cf22831c593d07339b90fb775a6895e Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Sun, 26 May 2024 21:42:04 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=BE=BE=E5=A6=99=E7=94=B5?= =?UTF-8?q?=E6=9C=BA=E6=8E=A7=E5=88=B6=E6=96=B9=E5=BC=8F=20=20=E4=B8=8D?= =?UTF-8?q?=E5=86=8D=E4=B8=BA=E6=AF=8F=E4=B8=AA=E7=94=B5=E6=9C=BA=E5=8D=95?= =?UTF-8?q?=E7=8B=AC=E5=88=9B=E5=BB=BA=E4=BB=BB=E5=8A=A1=EF=BC=8C=E8=A7=A3?= =?UTF-8?q?=E5=86=B3=E4=BA=86=E7=94=B5=E6=9C=BA=E6=A6=82=E7=8E=87=E5=8F=91?= =?UTF-8?q?=E6=8A=96=E3=80=81=E7=A6=BB=E7=BA=BF=E7=9A=84bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/cmd/robot_cmd.c | 69 ++++++++++++---------- application/gimbal/gimbal.cpp | 49 +++++++++------- application/robot_def.h | 2 +- application/robot_task.h | 4 +- bsp/can/bsp_can.c | 8 ++- engineering.jdebug | 2 +- engineering.jdebug.user | 71 ++++++++++++---------- modules/motor/DMmotor/dmmotor.c | 101 +++++++++++++++++++++++++++----- modules/motor/DMmotor/dmmotor.h | 1 + modules/motor/motor_task.c | 9 ++- 10 files changed, 211 insertions(+), 105 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 2383077..4a2b42b 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -71,7 +71,7 @@ void RobotCMDInit() { //vision_recv_data = VisionInit(&huart1); // 视觉通信串口 referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI - referee_VT_data = VTInit(&huart1); +// referee_VT_data = VTInit(&huart1); air_pump = PumpInit(&htim1,TIM_CHANNEL_1); @@ -145,7 +145,7 @@ static void RemoteControlSet() { chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转 - to_stretch_cmd_send.tc = 3.0F * (float) rc_data[TEMP].rc.rocker_l1;//图传 + to_stretch_cmd_send.tc = -3.0F * (float) rc_data[TEMP].rc.rocker_l1;//图传 // 图传限位 // if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE; // if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE; @@ -181,21 +181,21 @@ static void RemoteControlSet() { to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_r1;//前伸 } -// // 左侧开关状态为[中],后三轴 -// if(switch_is_mid(rc_data[TEMP].rc.switch_left)) -// { -// gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE; -// gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_; -// gimbal_cmd_send.diff_pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1; -// gimbal_cmd_send.diff_roll += 0.001f * (float) rc_data[TEMP].rc.rocker_r_; -// } - // 左侧开关状态为[中],逆解模式 + // 左侧开关状态为[中],后三轴 if(switch_is_mid(rc_data[TEMP].rc.switch_left)) { - gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; - gimbal_cmd_send.pitch = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制 - chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 + gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE; + gimbal_cmd_send.roll = 0.001f * (float) rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.diff_pitch = -0.001f * (float) rc_data[TEMP].rc.rocker_r1; + gimbal_cmd_send.diff_roll = 0.001f * (float) rc_data[TEMP].rc.rocker_r_; } + // 左侧开关状态为[中],逆解模式 +// if(switch_is_mid(rc_data[TEMP].rc.switch_left)) +// { +// gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; +// gimbal_cmd_send.pitch = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制 +// chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 +// } } //自定义控制器控制末端姿态 @@ -273,15 +273,15 @@ static void MouseKeySet() { break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V 开启自定义控制器 - { - case 1: - gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; //自定义 - rc_data[TEMP].key_count[KEY_PRESS][Key_V]++; - break; - default: - break; - } +// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V 开启自定义控制器 +// { +// case 1: +// gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; //自定义 +// rc_data[TEMP].key_count[KEY_PRESS][Key_V]++; +// break; +// default: +// break; +// } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) //B 切手动 { case 0: @@ -335,11 +335,20 @@ static void MouseKeySet() { to_stretch_cmd_send.fb += (float)(rc_data[TEMP].key[KEY_PRESS].z - rc_data[TEMP].key[KEY_PRESS].c) * 0.3f; } - chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 8000 - rc_data[TEMP].key[KEY_PRESS].s * 8000; // 系数待测 - chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 8000 - rc_data[TEMP].key[KEY_PRESS].d * 8000; + if(rc_data[TEMP].key[KEY_PRESS].shift) //shift 加速 + { + chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 8000 * 3 - rc_data[TEMP].key[KEY_PRESS].s * 8000 * 3; // 系数待测 + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 8000 * 3- rc_data[TEMP].key[KEY_PRESS].d * 8000 * 3; + } + else + { + chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 8000 - rc_data[TEMP].key[KEY_PRESS].s * 8000 ; // 系数待测 + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 8000 - rc_data[TEMP].key[KEY_PRESS].d * 8000 ; + } + chassis_cmd_send.wz = -(float) rc_data[TEMP].mouse.x * 8; - to_stretch_cmd_send.tc = -(float) rc_data[TEMP].mouse.y * 20; + to_stretch_cmd_send.tc = (float) rc_data[TEMP].mouse.y * 20; gimbal_cmd_send.pitch = (float)(rc_data[TEMP].key[KEY_PRESS].r - rc_data[TEMP].key[KEY_PRESS].f) * 0.25f; //增量控制 } @@ -382,7 +391,7 @@ void RobotCMDTask() { SubGetMessage(to_stretch_feed_sub, &to_stretch_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); - update_ui_data(); + // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 //CalcOffsetAngle(); // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 @@ -392,8 +401,6 @@ void RobotCMDTask() { else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 MouseKeySet(); - - EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 //裁判系统断电则不使能 @@ -412,7 +419,7 @@ void RobotCMDTask() { if(to_stretch_cmd_send.to_stretch_mode == TO_STRETCH_ZERO_FORCE) { to_stretch_cmd_send.fb = to_stretch_fetch_data.protract_x; - to_stretch_cmd_send.ud = to_stretch_fetch_data.lift_z; + to_stretch_cmd_send.ud = UD_MIN;//to_stretch_fetch_data.lift_z; } //抬升软件限位 if(to_stretch_cmd_send.ud <= UD_MIN) to_stretch_cmd_send.ud = UD_MIN; @@ -424,6 +431,8 @@ void RobotCMDTask() { // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 + + update_ui_data(); #ifdef ONE_BOARD PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send); #endif // ONE_BOARD diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index bff8228..09c7b93 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -158,13 +158,13 @@ void GimbalInit() .speed_PID = { .Kp = 0.4f, .Ki = 0.0f, - .Kd = 0.02f, + .Kd = 0.01f,//0.02f, .MaxOut = 10, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { - .Kp = 25.0f, + .Kp = 15.0f,//25.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10, @@ -225,7 +225,7 @@ void GimbalInit() first_order_filter_init(&Qset_filter[2],5e-3,&Qset_filter_num); PID_Init_Config_s diff_pitch_config= { - .Kp = 20.0f,//20.0F,//15.0f, + .Kp = 30.0f,//20.0F,//15.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, @@ -235,7 +235,7 @@ void GimbalInit() PIDInit(&diff_pitch_loop,&diff_pitch_config); PID_Init_Config_s diff_roll_config= { - .Kp = 14.0f, + .Kp = 50.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, @@ -284,7 +284,7 @@ void GimbalInit() gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); - //上电校准两个4310零点 +// //上电校准两个4310零点 // DMMotorCaliEncoder(diff_r_motor); // DMMotorCaliEncoder(diff_l_motor); ////水平方向3.02 矿仓 0 @@ -311,7 +311,7 @@ void GimbalInit() /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ -float debug_diff_kp = 0.3f,debug_diff_ki=0,debug_diff_kd = 0.01f; +float debug_diff_kp = 0.2f,debug_diff_ki=0,debug_diff_kd = 0.01f; float diff_motor_r_offset = 0; float diff_motor_l_offset = 0; void GimbalTask() { @@ -342,8 +342,8 @@ void GimbalTask() first_order_filter_cali(&diff_l_spd_filter,diff_l_motor->measure.velocity); diff_l_spd = diff_l_spd_filter.out; - float diff_r_angle = diff_r_motor->measure.position - diff_motor_r_offset; - float diff_l_angle = diff_l_motor->measure.position - diff_motor_l_offset; + float diff_r_angle = diff_r_motor->measure.total_angle - diff_motor_r_offset; + float diff_l_angle = diff_l_motor->measure.total_angle - diff_motor_l_offset; float diff_pitch_angle = (diff_r_angle + (-diff_l_angle))/2; float diff_roll_angle = (diff_r_angle - (-diff_l_angle))/2 * 18/52; @@ -366,27 +366,31 @@ void GimbalTask() // ik_q_cmd = ik_q.block<5,1>(0,0); // ik_q3 = robotics::spherical_wrist_ikine(robotics::t2r(fk_T),arm_q[0][0],arm_q[1][0]); + static uint8_t last_zero_flag = 0; + if(gimbal_cmd_recv.set_zero_flag != last_zero_flag) + { + diff_motor_r_offset = diff_r_motor->measure.total_angle; + diff_motor_l_offset = diff_l_motor->measure.total_angle; + + PIDClear(&diff_pitch_loop); + PIDClear(&diff_roll_loop); + PIDClear(&diff_r_motor->motor_controller.speed_PID); + PIDClear(&diff_l_motor->motor_controller.speed_PID); + //目标值也清零 + q_set[3] = 0 ; + q_set[4] = 0 ; + } + last_zero_flag = gimbal_cmd_recv.set_zero_flag; + if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) { - static uint8_t last_zero_flag = 0; + DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r); DMMotorStop(yaw_motor);DMMotorStop(roll_motor); DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor); - if(gimbal_cmd_recv.set_zero_flag != last_zero_flag) - { - diff_motor_r_offset = diff_r_motor->measure.position; - diff_motor_l_offset = diff_l_motor->measure.position; - - PIDClear(&diff_pitch_loop); - PIDClear(&diff_roll_loop); - PIDClear(&diff_r_motor->motor_controller.speed_PID); - PIDClear(&diff_l_motor->motor_controller.speed_PID); - } - last_zero_flag = gimbal_cmd_recv.set_zero_flag; - //不使能模式下 目标值为当前值 防止使能甩大臂 q_set[1] = pitch_motor_l->measure.position; } @@ -519,7 +523,8 @@ void GimbalTask() q_set[2] = float_constrain(q_set[2],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD); q_set[3] = float_constrain(q_set[3],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD); - q_set[4] = float_constrain(q_set[4],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD); + //roll轴不做限位处理 + //q_set[4] = float_constrain(q_set[4],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD); float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[3]); diff --git a/application/robot_def.h b/application/robot_def.h index 219ef49..7d8e9bd 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -36,7 +36,7 @@ #define YAW 60 //龙门架限位 -#define UD_MAX 210 +#define UD_MAX 151 #define UD_MIN 0 #define FB_MIN 0 diff --git a/application/robot_task.h b/application/robot_task.h index 50adf39..ffa1613 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -52,9 +52,9 @@ void OSTaskInit() osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512); uiTaskHandle = osThreadCreate(osThread(uitask), NULL); - HTMotorControlInit(); // 没有注册HT电机则不会执行 + // HTMotorControlInit(); // 没有注册HT电机则不会执行 - DMMotorControlInit(); // 没有注册DM电机则不会执行 + // DMMotorControlInit(); // 没有注册DM电机则不会执行 } __attribute__((noreturn)) void StartINSTASK(void const *argument) diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 15bfdda..e4bd430 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -112,7 +112,13 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout) { if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时 { - LOGWARNING("[bsp_can] CAN MAILbox full! failed to add msg to mailbox."); + if(_instance->can_handle == &hcan1) + LOGERROR("[}bsp_can] CAN1 MAILbox full! ,tx [%d] and rx [%d] full", &_instance->tx_id, &_instance->rx_id); + + //LOGWARNING("[bsp_can] CAN MAILbox full! failed to add msg to mailbox."); + if(_instance->can_handle == &hcan2) + LOGERROR("[}bsp_can] CAN2 MAILbox full! ,tx [%d] and rx [%d] full", &_instance->tx_id, &_instance->rx_id); + busy_count++; return 0; } diff --git a/engineering.jdebug b/engineering.jdebug index e536e07..708c732 100644 --- a/engineering.jdebug +++ b/engineering.jdebug @@ -33,7 +33,7 @@ void OnProjectLoad (void) { // // User settings // - Edit.SysVar (VAR_HSS_SPEED, FREQ_200_HZ); + Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ); Project.SetOSPlugin ("FreeRTOSPlugin_CM4"); File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf"); } diff --git a/engineering.jdebug.user b/engineering.jdebug.user index 4f2a4fa..3dfdfd9 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,42 +1,43 @@ -Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:280:5, State=BP_STATE_DISABLED -GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f -GraphedExpression="(lift_position_loop).Measure", Color=#35792b -OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141 -OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=195 -OpenDocument="referee_UI.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.c", Line=9 -OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=70 -OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=270 -OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=108 -OpenDocument="referee_UI.h", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.h", Line=0 -OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107 -OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=213 -OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=52 -OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=316 -OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=29 +Breakpoint=D:/CLion/Project/engineering-newDM/application/gimbal/gimbal.cpp:280:5, State=BP_STATE_DISABLED +OpenDocument="main.c", FilePath="D:/CLion/Project/engineering-newDM/Src/main.c", Line=71 +OpenDocument="bsp_can.c", FilePath="D:/CLion/Project/engineering-newDM/bsp/can/bsp_can.c", Line=151 +OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering-newDM/Startup/startup_stm32f407ighx.s", Line=53 +OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering-newDM/modules/referee/referee_task.c", Line=189 +OpenDocument="referee_UI.c", FilePath="D:/CLion/Project/engineering-newDM/modules/referee/referee_UI.c", Line=9 +OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering-newDM/bsp/dwt/bsp_dwt.c", Line=28 +OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering-newDM/application/cmd/robot_cmd.c", Line=375 +OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering-newDM/application/to_stretch/to_stretch.c", Line=108 +OpenDocument="referee_UI.h", FilePath="D:/CLion/Project/engineering-newDM/modules/referee/referee_UI.h", Line=0 +OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering-newDM/Src/stm32f4xx_it.c", Line=107 +OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering-newDM/modules/algorithm/controller.c", Line=141 +OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering-newDM/application/gimbal/gimbal.cpp", Line=245 +OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering-newDM/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=316 OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59 -OpenDocument="robot_task.h", FilePath="D:/CLion/Project/engineering/application/robot_task.h", Line=109 -OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3641 -OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1255 -OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=355 -OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412 -OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165 +OpenDocument="robot_task.h", FilePath="D:/CLion/Project/engineering-newDM/application/robot_task.h", Line=99 +OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering-newDM/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3641 +OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering-newDM/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1161 +OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/engineering-newDM/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=355 +OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering-newDM/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1409 +OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering-newDM/application/chassis/chassis.c", Line=165 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=358, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=273, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=687, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=979, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=611, h=632, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="312;24", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;45" -OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=252, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=531, h=224, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=516, h=260, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=516, h=256, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=679, h=407, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=801, h=407, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=531, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="329;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="343;0" +OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=438, h=407, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (lift_position_loop).Ref";" (lift_position_loop).Measure"], 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=[341;100;110;110;102;110;100;110;107] +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=[341;100;100;100;100;100;100;107;107] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;100;100] +TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;292] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[290;177;104;100;100] 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="arm_q.data_", RefreshRate=2, Window=Watched Data 1 @@ -69,4 +70,12 @@ WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1 WatchedExpression="tuchuan", RefreshRate=2, Window=Watched Data 1 WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1 WatchedExpression="rc_data[TEMP].key_count[KEY_PRESS][Key_Shift]", Window=Watched Data 1 -WatchedExpression="rc_data", Window=Watched Data 1 \ No newline at end of file +WatchedExpression="rc_data", Window=Watched Data 1 +WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_roll_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_pitch_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_l_motor", Window=Watched Data 1 +WatchedExpression="debug_diff_kp", Window=Watched Data 1 +WatchedExpression="debug_diff_kd", Window=Watched Data 1 +WatchedExpression="debug_diff_ki", Window=Watched Data 1 +WatchedExpression="roll_motor", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 2c78d0f..8abaea3 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -52,7 +52,8 @@ static void DMMotorDecode(CANInstance *motor_can) tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); - measure->angle_single_round = RAD_2_DEGREE * (measure->position+3.141593); + //measure->angle_single_round = RAD_2_DEGREE * (measure->position+3.141593); + measure->angle_single_round = (measure->position); tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4); measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); @@ -69,7 +70,8 @@ static void DMMotorDecode(CANInstance *motor_can) else if (measure->position - measure->last_position < -PI) measure->total_round++; - measure->total_angle = measure->total_round * (DM_P_MAX * DEGREE_2_RAD) + measure->angle_single_round; + //measure->total_angle = measure->total_round * (DM_P_MAX * DEGREE_2_RAD) + measure->angle_single_round; + measure->total_angle = measure->total_round * (DM_P_MAX*2) + measure->angle_single_round; } static void DMMotorLostCallback(void *motor_ptr) @@ -154,6 +156,8 @@ void DMMotorTask(void const *argument) { float pid_ref, set, pid_measure; + + DMMotorInstance *motor = (DMMotorInstance *)argument; Motor_Controller_s *motor_controller; // 电机控制器 DM_Motor_Measure_s *measure = &motor->measure; @@ -231,18 +235,87 @@ void DMMotorTask(void const *argument) osDelay(5); } } -void DMMotorControlInit() -{ - char dm_task_name[5] = "dm"; - // 遍历所有电机实例,创建任务 - if (!idx) - return; - for (size_t i = 0; i < idx; i++) + + +void DMMotorControl(){ + DMMotorInstance *motor; + Motor_Controller_s *motor_controller; // 电机控制器 + DM_Motor_Measure_s *measure; + Motor_Control_Setting_s *setting; + //CANInstance *motor_can = motor->motor_can_instance; + //uint16_t tmp; + DMMotor_Send_s motor_send_mailbox; + float pid_ref, set, pid_measure; + + for (size_t i = 0; i < idx; ++i) { - char dm_id_buff[2] = {0}; - __itoa(i, dm_id_buff, 10); - strcat(dm_task_name, dm_id_buff); - osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128); - dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]); + motor = dm_motor_instance[i]; + setting = &motor->motor_settings; + motor_controller = &motor->motor_controller; + measure = &motor->measure; + + pid_ref = motor->motor_controller.pid_ref;//保存设定值 + + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref *= -1; + + // pid_ref会顺次通过被启用的闭环充当数据的载体 + // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 + if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP) + { + if (setting->angle_feedback_source == OTHER_FEED) + pid_measure = *motor_controller->other_angle_feedback_ptr; + else + pid_measure = measure->position; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃 + // 更新pid_ref进入下一个环 + pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref); + } + + // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环 + + + if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) + { + if (setting->feedforward_flag & SPEED_FEEDFORWARD) + pid_ref += *motor_controller->speed_feedforward_ptr; + if (setting->speed_feedback_source == OTHER_FEED) + pid_measure = *motor_controller->other_speed_feedback_ptr; + else // MOTOR_FEED + pid_measure = measure->velocity; + // 更新pid_ref进入下一个环 + pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); + } + // 电流环前馈 + if (setting->feedforward_flag & CURRENT_FEEDFORWARD) + pid_ref += *motor_controller->current_feedforward_ptr; + + if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) + pid_ref *= -1; + + set = pid_ref; + + if (motor->stop_flag == MOTOR_STOP) + set = 0; + + LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX); + motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16); + motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12); + motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12); + motor_send_mailbox.Kp = 0;//只使用力矩控制无需kpkd参数 + motor_send_mailbox.Kd = 0;//只使用力矩控制无需kpkd参数 + + + motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8); + motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des); + motor->motor_can_instance->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4); + motor->motor_can_instance->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8)); + motor->motor_can_instance->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp); + motor->motor_can_instance->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4); + motor->motor_can_instance->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8)); + motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des); + + + CANTransmit(motor->motor_can_instance, 1); } + } diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index 310713d..d04e6c5 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -72,4 +72,5 @@ void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor); void DMMotorStop(DMMotorInstance *motor); void DMMotorCaliEncoder(DMMotorInstance *motor); void DMMotorControlInit(); +void DMMotorControl(); #endif // !DMMOTOR \ No newline at end of file diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 17c281e..0a714dd 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -4,17 +4,20 @@ #include "dji_motor.h" #include "step_motor.h" #include "servo_motor.h" +#include "dmmotor.h" void MotorControlTask() { - // static uint8_t cnt = 0; 设定不同电机的任务频率 - // if(cnt%5==0) //200hz + static uint8_t cnt = 0; //设定不同电机的任务频率 + if(cnt%5==0) //200hz + DJIMotorControl(); // if(cnt%10==0) //100hz - DJIMotorControl(); + DMMotorControl(); /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ //LKMotorControl(); + // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换