TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-23 17:42:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

damiao motor driver init_1

Browse Source
This commit is contained in:
Zorua
2026-03-15 12:52:05 +08:00
committed by guoxy06
parent 9d4e103a01
commit 4b17e1e193
7 changed files with 53 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
application/chassis/chassis.c
View File

@@ -113,7 +113,7 @@ void ChassisInit()
.can_init_config.can_handle = &hcan1, .can_init_config.can_handle = &hcan1,
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 50,//15 .Kp = 12,
.Ki = 0.2, .Ki = 0.2,
.Kd = 0, .Kd = 0,
.CoefA = 3, .CoefA = 3,
@@ -125,7 +125,7 @@ void ChassisInit()
.DeadBand = 0.5, .DeadBand = 0.5,
}, },
.speed_PID = { .speed_PID = {
.Kp = 50,//25 .Kp = 40,
.Ki = 3, .Ki = 3,
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | PID_OutputFilter, .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | PID_OutputFilter,
@@ -154,7 +154,7 @@ void ChassisInit()
.Kp = 1.05, // 6 .Kp = 1.05, // 6
.Ki = 0, .Ki = 0,
.Kd = 0.1, // 0.495 .Kd = 0.1, // 0.495
// .DeadBand = 0.5f,// 启用死区，值为0.5度 .DeadBand = 0.5f,// 启用死区，值为0.5度
//.CoefA = 0.2, //.CoefA = 0.2,
//.CoefB = 0.3, //.CoefB = 0.3,
//.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, //.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
@@ -412,9 +412,7 @@ void ChassisTask()
{ {
single_round_offset += 360.0f; single_round_offset += 360.0f;
} }
chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
// chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
// chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100; // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100;
break; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略

28
application/cmd/robot_cmd.c
View File

@@ -84,9 +84,9 @@ void RobotCMDInit()
static void CalcOffsetAngle() static void CalcOffsetAngle()
{ {
static float last_offset_angle = 0; static float last_offset_angle = 0;
const float smooth_factor = 0.2f; // 平滑因子,0~1之间,值越大平滑效果越明显 const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显
//const float deadband = 3.0f; // 死区值 const float deadband = 3.0f; // 死区值
// 新增：跟踪模式变化，用于重置基准角度 // 新增：跟踪模式变化，用于重置基准角度
// static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;// // static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;//
// //
@@ -108,14 +108,14 @@ static void CalcOffsetAngle()
else if (angle_diff < -180.0f) else if (angle_diff < -180.0f)
new_offset += 360.0f; new_offset += 360.0f;
///// /////
// // 应用死区 // 应用死区
// if (fabs(new_offset) < deadband) { if (fabs(new_offset) < deadband) {
// new_offset = 0.0f; new_offset = 0.0f;
// } else if (new_offset > 0) { } else if (new_offset > 0) {
// new_offset -= deadband; new_offset -= deadband;
// } else { } else {
// new_offset += deadband; new_offset += deadband;
// }////// }//////
chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor; chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor;
last_offset_angle = chassis_cmd_send.offset_angle; last_offset_angle = chassis_cmd_send.offset_angle;
// // 更新模式状态 // // 更新模式状态
@@ -203,14 +203,14 @@ static void RemoteControlSet()
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
if (switch_is_down(rc_data[TEMP].rc.switch_left) ) if (switch_is_down(rc_data[TEMP].rc.switch_left) )
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整 { // 按照摇杆的输出大小进行角度增量,增益系数需调整
gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_r_; gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_r1; gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
} }
// 云台软件限位 // 云台软件限位
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_l_; // _水平方向 chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_l1; // 1数值方向 chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
// // 发射参数 // // 发射参数
// if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开

15
application/gimbal/gimbal.c
View File

@@ -66,7 +66,7 @@ void GimbalInit()
Motor_Init_Config_s pitch_config = { Motor_Init_Config_s pitch_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
.tx_id = 2,//2 .tx_id = 4,//2
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
@@ -79,20 +79,12 @@ void GimbalInit()
}, },
.speed_PID = { .speed_PID = {
.Kp = 50, // 50 .Kp = 50, // 50
.Ki = 175, // 350 .Ki = 350, // 350
.Kd = 0, // 0 .Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500, .IntegralLimit = 2500,
.MaxOut = 12000, .MaxOut = 20000,
}, },
// .current_PID = { // 添加current_PID配置
// .Kp = 0,
// .Ki = 0,
// .Kd = 0,
// .Improve = PID_Integral_Limit,
// .IntegralLimit = 5000,
// .MaxOut = 15000,
// },
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
@@ -103,7 +95,6 @@ void GimbalInit()
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
}, },
.motor_type = DM4310, .motor_type = DM4310,
}; };

4
application/robot_def.h
View File

@@ -208,10 +208,6 @@ typedef struct
attitude_t gimbal_imu_data; attitude_t gimbal_imu_data;
uint16_t yaw_motor_single_round_angle; uint16_t yaw_motor_single_round_angle;
uint16_t yaw_motor_total_angle; uint16_t yaw_motor_total_angle;
//uint16_t pitch_motor_single_round_angle;
//uint16_t pitch_motor_total_angle;
// 后续增加云台的其他数据
// ...
} Gimbal_Upload_Data_s; } Gimbal_Upload_Data_s;
typedef struct typedef struct

2
application/shoot/shoot.c
View File

@@ -55,7 +55,7 @@ void ShootInit()
friction_config.can_init_config.tx_id = 1, friction_config.can_init_config.tx_id = 1,
friction_l = DJIMotorInit(&friction_config); friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行 friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_r = DJIMotorInit(&friction_config); friction_r = DJIMotorInit(&friction_config);

47
modules/motor/DJImotor/dji_motor.c
View File

@@ -20,7 +20,6 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/ */
static CANInstance sender_assignment[6] = { static CANInstance sender_assignment[6] = {
//[0] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
@@ -52,12 +51,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
if (motor_id < 4) // 根据ID分组 if (motor_id < 4) // 根据ID分组
{ {
motor_send_num = motor_id; motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan2 ? 1 : 4; motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
} }
else else
{ {
motor_send_num = motor_id - 4; motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan2 ? 0 : 3; motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
} }
// 计算接收id并设置分组发送id // 计算接收id并设置分组发送id
@@ -72,7 +71,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{ {
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan2 ? 1 : 2; uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是) while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus); LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
} }
@@ -91,6 +90,22 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
motor_grouping = config->can_handle == &hcan1 ? 2 : 5; motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
} }
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
case DM4310: case DM4310:
if (motor_id < 4) // 根据ID分组 if (motor_id < 4) // 根据ID分组
{ {
@@ -292,34 +307,34 @@ void DJIMotorControl()
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
} }
//计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD) if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr; pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP) if (motor_setting->close_loop_type & CURRENT_LOOP)
{ {
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
} }
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1; pid_ref *= -1;
// 获取最终输出 // 获取最终输出
set = (int16_t)pid_ref; set = (int16_t)pid_ref;
// 分组填入发送数据 // 分组填入发送数据
group = motor->sender_group; group = motor->sender_group;
num = motor->message_num; num = motor->message_num;
//DM电机单独处理,因为它的发送格式与DJI电机不同,但仍然使用sender_assignment进行分组发送
if (i == 1) if (i == 1)
{ {
sender_assignment[4].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位 sender_assignment[4].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位
sender_assignment[4].tx_buff[7] = (uint8_t)(set >> 8); // 低八位 sender_assignment[4].tx_buff[7] = (uint8_t)(set >> 8); // 低八位
} }
else else
{ {
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位 sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位 sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
} }
// 若该电机处于停止状态,直接将buff置零 // 若该电机处于停止状态,直接将buff置零

1
modules/motor/DJImotor/dji_motor.h
View File

@@ -105,7 +105,6 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
*/ */
void DJIMotorControl(); void DJIMotorControl();
void DM_MotorControl();
/** /**
* @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零 * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
* *