diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 0504d2f..4f84e58 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -223,14 +223,14 @@ static void RemoteControlSet() if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺 { shoot_cmd_send.friction_mode = FRICTION_ON; -// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; -// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台 { shoot_cmd_send.friction_mode = FRICTION_OFF; - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } // 云台参数,确定云台控制数据 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 1442a93..d315a7a 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -6,9 +6,11 @@ #include "general_def.h" #include "bmi088.h" +#include "dm4310.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 -static DJIMotorInstance *yaw_motor, *pitch_motor; +static DJIMotorInstance *pitch_motor; +static DMMotorInstance *yaw_motor; static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 @@ -22,22 +24,23 @@ void GimbalInit() // YAW Motor_Init_Config_s yaw_config = { .can_init_config = { - .can_handle = &hcan2, - .tx_id = 1, + .can_handle = &hcan1, + .tx_id = 0x01, + .rx_id = 0x43, }, .controller_param_init_config = { .angle_PID = { - .Kp = 4, // 8 + .Kp = 0.5, // 8 .Ki = 0, - .Kd = 0.3, + .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { - .Kp = 5000, // 70 - .Ki = 1000, // 200 + .Kp = 0.5f, // 70 + .Ki = 0.2f, // 200 .Kd = 0,//10 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 3000, @@ -48,13 +51,13 @@ void GimbalInit() .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP |SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, - .motor_type = GM6020 + .motor_type = DM4310 }; // PITCH Motor_Init_Config_s pitch_config = { @@ -94,7 +97,7 @@ void GimbalInit() .motor_type = M3508, }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 - yaw_motor = DJIMotorInit(&yaw_config); + yaw_motor = DMMotorInit(&yaw_config); pitch_motor = DJIMotorInit(&pitch_config); gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); @@ -118,41 +121,28 @@ void GimbalTask() { // 停止 case GIMBAL_ZERO_FORCE: - DJIMotorStop(yaw_motor); + DMMotorStop(yaw_motor); DJIMotorStop(pitch_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DJIMotorEnable(yaw_motor); + DMMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - case ADJUST_PITCH_MODE: - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - horizontal_check(); +// DJIMotorEnable(yaw_motor); +// DJIMotorEnable(pitch_motor); +// DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); +// DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); +// DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); +// DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); +// DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 +// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; default: break; diff --git a/application/robot_def.h b/application/robot_def.h index 1763aef..f971567 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -199,7 +199,7 @@ typedef struct typedef struct { attitude_t gimbal_imu_data; - uint16_t yaw_motor_single_round_angle; + float yaw_motor_single_round_angle; float pitch_motor_total_angle; } Gimbal_Upload_Data_s; diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 79fb4ca..0225ba2 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -156,9 +156,8 @@ void ShootTask() { // 停止拨盘 case LOAD_STOP: - DJIMotorStop(loader); -// DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 -// DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 + DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 + DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 break; // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index b41ddb9..2cb1a81 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -168,7 +168,6 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等 // motor controller init 电机控制器初始化 - PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID); PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID); PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; diff --git a/modules/motor/DMmotor/dm4310.c b/modules/motor/DMmotor/dm4310.c index bcc8f39..d3849f0 100644 --- a/modules/motor/DMmotor/dm4310.c +++ b/modules/motor/DMmotor/dm4310.c @@ -47,9 +47,7 @@ static void DMMotorDecode(CANInstance *motor_can) measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); measure->angle_single_round = ECD_ANGLE_COEF_DM * (measure->position+3.141593); - if(measure->angle_single_round > 360) measure->angle_single_round -= 360; - if(measure->angle_single_round < 0) measure->angle_single_round += 360 ; - + tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4); measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); @@ -58,6 +56,14 @@ static void DMMotorDecode(CANInstance *motor_can) measure->T_Mos = (float)rxbuff[6]; measure->T_Rotor = (float)rxbuff[7]; + + // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了 + if (measure->position - measure->last_position > 3.1425926) + measure->total_round--; + else if (measure->position - measure->last_position < -3.1415926) + measure->total_round++; + + measure->total_angle = measure->total_round * 360 + measure->angle_single_round; } static void DMMotorLostCallback(void *motor_ptr) @@ -76,12 +82,14 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance)); memset(motor, 0, sizeof(DMMotorInstance)); + config->can_init_config.rx_id = config->can_init_config.rx_id; + config->can_init_config.tx_id = config->can_init_config.tx_id; motor->motor_settings = config->controller_setting_init_config; - PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); - PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); - PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); - motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; - motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + PIDInit(&motor->motor_controller.current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&motor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&motor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); + motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; config->can_init_config.can_module_callback = DMMotorDecode; config->can_init_config.id = motor; @@ -103,7 +111,7 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) void DMMotorSetRef(DMMotorInstance *motor, float ref) { - motor->pid_ref = ref; + motor->motor_controller.pid_ref = ref; } void DMMotorEnable(DMMotorInstance *motor) @@ -122,37 +130,69 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type) } -//位置控制 +//还得使用力矩控制 void DMMotorTask(void const *argument) { - float pid_ref, set; + float pid_ref, set, pid_measure; + DMMotorInstance *motor = (DMMotorInstance *)argument; - //DM_Motor_Measure_s *measure = &motor->measure; + Motor_Controller_s *motor_controller; // 电机控制器 + DM_Motor_Measure_s *measure = &motor->measure; + motor_controller = &motor->motor_controller; Motor_Control_Setting_s *setting = &motor->motor_settings; //CANInstance *motor_can = motor->motor_can_instance; //uint16_t tmp; DMMotor_Send_s motor_send_mailbox; while (1) { - pid_ref = motor->pid_ref; + 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->total_angle; // 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->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) + pid_ref *= -1; set = pid_ref; - if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) - set *= -1; + + 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(set, DM_V_MIN, DM_V_MAX, 12); - motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12); - motor_send_mailbox.Kp = motor->angle_PID.Kp; - motor_send_mailbox.Kd = motor->angle_PID.Kd; + 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参数 + - if(motor->stop_flag == MOTOR_STOP) - { - 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(0, DM_T_MIN, DM_T_MAX, 12); - } 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); @@ -163,6 +203,9 @@ void DMMotorTask(void const *argument) 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); osDelay(2); diff --git a/modules/motor/DMmotor/dm4310.h b/modules/motor/DMmotor/dm4310.h index cbbbb78..12d9ff0 100644 --- a/modules/motor/DMmotor/dm4310.h +++ b/modules/motor/DMmotor/dm4310.h @@ -8,8 +8,8 @@ #define DM_MOTOR_CNT 1 -#define DM_P_MIN (-12.5f) -#define DM_P_MAX 12.5f +#define DM_P_MIN (-3.1415926f) +#define DM_P_MAX 3.1415926f #define DM_V_MIN (-45.0f) #define DM_V_MAX 45.0f #define DM_T_MIN (-18.0f) @@ -20,14 +20,15 @@ typedef struct { uint8_t id; uint8_t state; - float velocity; - float last_position; - float position; - float torque; - float T_Mos; - float T_Rotor; - float angle_single_round; - int32_t total_round; + float velocity; //速度 + float last_position; //上次位置 + float position; //位置 + float torque; //力矩 + float T_Mos; //mos温度 + float T_Rotor; //电机温度 + float angle_single_round; //单圈角度 + int32_t total_round; //总圈数 + float total_angle; //总角度 }DM_Motor_Measure_s; typedef struct @@ -42,16 +43,13 @@ typedef struct { DM_Motor_Measure_s measure; Motor_Control_Setting_s motor_settings; - PIDInstance current_PID; - PIDInstance speed_PID; - PIDInstance angle_PID; - float *other_angle_feedback_ptr; - float *other_speed_feedback_ptr; - float *speed_feedforward_ptr; - float *current_feedforward_ptr; - float pid_ref; + Motor_Controller_s motor_controller;//电机控制器 + + CANInstance *motor_can_instance;//电机can实例 + + Motor_Type_e motor_type;//电机类型 Motor_Working_Type_e stop_flag; - CANInstance *motor_can_instance; + DaemonInstance* motor_daemon; uint32_t lost_cnt; }DMMotorInstance; diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index fdfad15..90f2f7b 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -116,6 +116,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config) config->can_init_config.can_module_callback = HTMotorDecode; config->can_init_config.id = motor; + config->can_init_config.rx_id = config->can_init_config.tx_id; motor->motor_can_instace = CANRegister(&config->can_init_config); Daemon_Init_Config_s conf = { diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index c33ce7e..d0d5e8d 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -109,6 +109,7 @@ typedef struct typedef enum { MOTOR_TYPE_NONE = 0, + DM4310, GM6020, M3508, M2006,