PID 控制算法

最新推荐文章于 2024-04-01 19:38:36 发布

OwnResponsibility

最新推荐文章于 2024-04-01 19:38:36 发布

阅读量471

点赞数 6

分类专栏：常用算法文章标签：算法

本文链接：https://blog.csdn.net/qq_38757779/article/details/135188990

版权

常用算法专栏收录该内容

1 篇文章 0 订阅

订阅专栏

会用就行

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;
}