会用就行
PID 控制算法
0、前期分析
主控发送值(控制器输出)
数据域 | 内容 | 电调ID |
---|---|---|
DATA[0] | 控制电流值高8位 | 1 |
DATA[1] | 控制电流值低8位 | 1 |
DATA[2] | 控制电流值高8位 | 2 |
DATA[3] | 控制电流值低8位 | 2 |
主控接收值(实际电机输出)
数据域 | 内容 |
---|---|
DATA[0] | 转子机械角度高8位 |
DATA[1] | 转子机械角度低8位 |
DATA[2] | 转子转速高8位 |
DATA[3] | 转子转速低8位 |
DATA[4] | 实际转矩电流高8位 |
DATA[5] | 实际转矩电流低8位 |
DATA[6] | 电机温度 |
DATA[7] | NULL |
上述值的发送与接受遵循CAN协议
1、RM电机M3580速度单环
2、代码讲解
classis_motor_pid[i].SpeedId,target=500;
classis_motor_pid[i].SpeedId.current=motor_chassis[i].speed_rpm;
pid_calc(&chassis_motor_pid[i].SpeedPID);
/*位置PID算法*/
void pid_calc(_pid* pid){
pid->e =pid->target-pid->current;
//倍数
pid->p_out=(int32_t)(pid->Kp*pid->e);
//积分分离
if(fabs(pid->e)<I_Band){
//积分
pid->i_out +=(int32_t)(pid->Ki*pid->e);
//积分限幅
limit(&(pid->i_out),pid->IntegralLimit);
}else{
pid->i_out=0;
}
pid->d_out=(int32_t)(pid->Kd*(pid->e-pid->last_e));
pid->total_out=pid->p_out+pid->i_out+pid->d_out;
//pid 输出限幅
limit(&(pid->total_out),pid->MaxOutput);
pid->last_e=pid->e;
}
2、RM电机6020角度双环
1、控制系统
2、代码
manipulator_motor_pid[i].AnglePID.target=30;//degree
update_angle(&manipulator_motor_pid[i]._angle,motor_manipulator[i].angle);
pid_calc(&manipulator_motor_pid[i].AnglePID);
manipulator_motor_pid[i].SpeedPID.target=manipulator_motor_pid[i].AnglePID.total_out;
manipulator_motor_pid[i].SpeedPID.current=motor_manipulator[i].speed_rpm;
pid_calc(&manipulator_motor_pid[i].SpeedPID);
/*位置pid算法*/
void pid_calc(_pid* pid){
pid->e =pid->target-pid->current;
//倍数
pid->p_out=(int32_t)(pid->Kp*pid->e);
//积分分离
if(fabs(pid->e)<I_Band){
//积分
pid->i_out +=(int32_t)(pid->Ki*pid->e);
//积分限幅
limit(&(pid->i_out),pid->IntegralLimit);
}else{
pid->i_out=0;
}
pid->d_out=(int32_t)(pid->Kd*(pid->e-pid->last_e));
pid->total_out=pid->p_out+pid->i_out+pid->d_out;
//pid 输出限幅
limit(&(pid->total_out),pid->MaxOutput);
pid->last_e=pid->e;
}
/*角度更新*/
void update_angle(motor_angle* _angle, uint16_t angle_fb){
_angle->encoder=angle_fb;
if(_angle->encoder_is_init){
if(_angle->encoder-_angle->last_encoder>4096){
_angle->round_cnt --;
}else if(_angle->encoder-_angle->last_encoder<-4096){
_angle->round_cont++;
}
}else{
_angle->encoder_offset=_angle->encoder;
_angle->encoder_is_init=1;
}
_angle->angle_offset=_angle->encoder_offset /8192.of *360.of;
_angle->last_encoder= _angle->encoder;
_angle->total_encoder=_angle->round_cnt *8192+_angle->encoder-_angle->encoder_offset;
_angle->angle=_angle->total_encoder / 8192.0f * 360.0f;
}