修改达妙电机控制方式 不再为每个电机单独创建任务，解决了电机概率发抖、离线的bug
This commit is contained in:
parent
7ec9ec3a98
commit
817f3a771c
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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_Improvement_e>(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]);
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
#define YAW 60
|
||||
|
||||
//龙门架限位
|
||||
#define UD_MAX 210
|
||||
#define UD_MAX 151
|
||||
#define UD_MIN 0
|
||||
|
||||
#define FB_MIN 0
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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中提供了以任务方式启动控制的接口,可通过宏定义切换
|
||||
|
|
|
|||
Loading…
Reference in New Issue