/** ****************************(C) COPYRIGHT 2021 QUT**************************** * @file motor_control.c/h * @brief 3508/2006电机初始化、速度环、位置环；6020电机初始化、位置环（相对角度） * @note 3508位置环使用的是增量式编码器，没有绝对位置，注意它所对应的角度是M3508的输出轴所对应的角度，由减速比经过换算得来， * 若要修改对于的真实角度，应该在CAN_RECEIVE中修改相对应的解码函数。 同时3508的控制函数也可对应相同ID的2006配套使用（但注意相对应的角度和速度值可能会出现问题）需要修改相应的减速比 * @history * Version Date Author Modification remark * V1.0.0 Dec-23-2021 QUT 1. 完成 * V1.0.1 Dec-23-2023 QUT hks 1. 完成 仅仅对原先函数进行了少量修改，增添了部分注释，未对部分参数进行适配 @verbatim ============================================================================== ============================================================================== @endverbatim ****************************(C) COPYRIGHT 2021 QUT**************************** */ #include "motor_control.h" #include "CAN_receive.h" #include "pid.h" #include "user_math.h" #include "device\device.h" //3508motor init ID1~4 //mode 1:velocity // 2:pos void M3508_motor_init(int mode); //3508velocity_loop ID1~4 void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4); //3508position_loop ID1~4 void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4); //6020 init ID1 void GM6020_position_loop_init(void); //6020 pos_loop ID1 relative angle ,this is used for 6020 angel range from 0 to 180 void GM6020_pos_loop(int32_t ecd); const motor_measure_t *motor_data[4];//3508 motor _data pid_type_def motor_pid[4];//3508velocity_pid pid_type_def angle_pid[4];//3508position_pid //position_pid time interval 10ms const fp32 speed_PID[3]={5.0,0.3,0.0}; //P,I,D(velocity) const fp32 angle_PID[3]={25.0,0.0,1.5}; //P,I,D(position) //velocity_pid const fp32 PID[3]={5.0,0.01,0.0}; const motor_measure_t *GM6020_data;//6020 data_motor gimbal_PID_t gimbal_motor_relative_angle_pid;//6020 relative angle position_pid pid_type_def gimbal_motor_gyro_pid;//6020 relative angle velocity_pid gimbal_pid_control_t GM6020_pos_loop_control;//6020 position_control const fp32 GM6020_PID[3]={3600.0,20.0,0};//6020 velocity_PID //*****3508*****// //3508 motor init ID1~4 //mode 1:velocity // 2:position void M3508_motor_init(int mode) { //get 3508 motor_data from CAN_receive motor_data[0] = get_chassis_motor_measure_point(0); motor_data[1] = get_chassis_motor_measure_point(1); motor_data[2] = get_chassis_motor_measure_point(2); motor_data[3] = get_chassis_motor_measure_point(3); switch(mode) { //velocity_init case 1: for(int i=0;i<4;i++) { PID_init(&motor_pid[i],PID_POSITION,PID,16000,2000); //velocity_pid init }break; //pos_init case 2: for(int i=0;i<4;i++) { PID_init(&motor_pid[i],PID_POSITION,speed_PID,16000,2000); //velocity_pid init PID_init(&angle_pid[i],PID_POSITION,angle_PID,1000,200); // pos_pid init }break; } } //3508 velocity /** * @brief send control current of motor (0x201, 0x202, 0x203, 0x204) * the param is compatable with the fab data of 3508 * @param[in] speed1: (0x201) 3508 motor control current, range [-16384,16384] * @param[in] speed2: (0x202) 3508 motor control current, range [-16384,16384] * @param[in] speed3: (0x203) 3508 motor control current, range [-16384,16384] * @param[in] speed4: (0x204) 3508 motor control current, range [-16384,16384] * @retval none */ void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4) { int32_t speed[4]; int32_t delta_speed[4]; int i; speed[0] = speed1; speed[1] = speed2; speed[2] = speed3; speed[3] = speed4; for(i=0;i<4;i++) { delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,speed[i]); } CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]); } //3508 pos /** * @brief send control current of motor (0x201, 0x202, 0x203, 0x204) * this also can used to control the same ID of M2006 * the param angle is correspond to the output shaft of M3508 * @Attention!!!!! the kind fo DJI ecd is incremental. * @param[in] angle1: (0x201) 3508 motor control current, range [reserved] * @param[in] angle2: (0x202) 3508 motor control current, range [reserved] * @param[in] angle3: (0x203) 3508 motor control current, range [reserved] * @param[in] angle4: (0x204) 3508 motor control current, range [reserved] * @retval none */ void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4) { fp32 angle[4]; fp32 delta_angle[4]; int32_t delta_speed[4]; int i; angle[0] = angle1; angle[1] = angle2; angle[2] = angle3; angle[3] = angle4; //outer ring angle_loop for(i=0;i<4;i++) { delta_angle[i] = PID_calc(&angle_pid[i],motor_data[i]->total_angle,angle[i]); } //inner ring velocity_loop for(i=0;i<4;i++) { delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,delta_angle[i]); } CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]); } //*****6020*****// //6020motor_init ID1 void GM6020_position_loop_init(void) { gimbal_PID_init(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,10.0f,0.0f,8.0f,5.0f,0.0f); //angle_loop_pid init PID_init(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,PID_POSITION,GM6020_PID,30000.0,5000.0); //velocity_loop init GM6020_data=get_G6020_motor_measure_point(); } //6020Pos ID1(relative angle) //this 6020is uesd only for angle_maximum(180) //ecd encoder valueֵ(0-8191) void GM6020_pos_loop(int32_t ecd) { //set positon GM6020_pos_loop_control.angel_set=rad(ecd); //calculate_relative_angle(-PI,PI) GM6020_pos_loop_control.angel_get=motor_ecd_to_angle_change(GM6020_data->ecd,GM6020_pos_loop_control.angel_set); //calculate_gyro (set-get) GM6020_pos_loop_control.gyro_get=rad(GM6020_pos_loop_control.angel_set-GM6020_pos_loop_control.angel_get); //angle_loop, pid GM6020_pos_loop_control.gyro_set = gimbal_PID_calc(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,GM6020_pos_loop_control.angel_get, GM6020_pos_loop_control.angel_set,GM6020_pos_loop_control.gyro_get); GM6020_pos_loop_control.current_set = PID_calc(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,GM6020_pos_loop_control.gyro_get, GM6020_pos_loop_control.gyro_set); //control value GM6020_pos_loop_control.current_given = (int16_t)(GM6020_pos_loop_control.current_set); CAN_cmd_gimbal(GM6020_pos_loop_control.current_given,0,0,0); } void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd) { // if (pid == NULL) // { // return; // } pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->err = 0.0f; pid->get = 0.0f; pid->max_iout = max_iout; pid->max_out = maxout; } fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta) { fp32 err; // if (pid == NULL) // { // return 0.0f; // } pid->get = get; pid->set = set; err = set - get; pid->err = rad_format(err); pid->Pout = pid->kp * pid->err; pid->Iout += pid->ki * pid->err; pid->Dout = pid->kd * error_delta; abs_limit_fp(&pid->Iout, pid->max_iout); pid->out = pid->Pout + pid->Iout + pid->Dout; abs_limit_fp(&pid->out, pid->max_out); return pid->out; } //calculate relative_angle (-4096,4095) fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd) { int32_t relative_ecd = ecd - offset_ecd; if (relative_ecd > 4095) { while(relative_ecd>4095) { relative_ecd -= 8191; } } else if (relative_ecd < -4096) { while(relative_ecd<-4096) { relative_ecd += 8191; } } return relative_ecd * MOTOR_ECD_TO_RAD; }