方向PID和小车控制

ooolmf

已于 2023-05-26 00:05:39 修改

阅读量906

点赞数 2

分类专栏：单片机 33 框架F4 文章标签：单片机

于 2023-05-26 00:05:14 首次发布

本文链接：https://blog.csdn.net/lmf666/article/details/130877802

版权

单片机同时被 3 个专栏收录

281 篇文章 135 订阅

订阅专栏

框架F4

6 篇文章 0 订阅

订阅专栏

4 篇文章 0 订阅

订阅专栏

该文描述了一个基于PID的小车控制算法，主要用于调整小车的前进速度和旋转速度。PID参数初始化后，通过计算误差和反馈值来调整控制输出。在循迹模式下，利用Z轴角速度作为误差输入，限制输出值在一定范围内。同时，根据小车前进和旋转的标记，动态设置前进速度和旋转角度，以实现精确控制。

摘要由CSDN通过智能技术生成

//方向PID，这里只用了PD
float direction_pid_point=0;
float direction_pid_kp=17;
float direction_pid_kd=2;
float direction_pid_uk=0;

void Direction_Pid_Parameter_Init(float set_point,float set_kp,float set_kd)
{
	direction_pid_point=set_point;
	direction_pid_kp=set_kp;
	direction_pid_kd=set_kd;
}

//			Direction_Pid_Calculate(error,-z_angular_speed);

void Direction_Pid_Calculate(float feedbackValue1,float feedbackValue2)
{
	float out=0;
  float direction_pid_ek=0;
	//计算误差
  direction_pid_ek=direction_pid_point-feedbackValue1;
  //计算输出值
  out=direction_pid_kp*direction_pid_ek+direction_pid_kd*feedbackValue2;
  //输出限幅
  if(out<-300) out=-300;
  else if(out>300) out=300;
	direction_pid_uk=out;
}



void Car_Move_Control(void)
{
	float set_forward_speed=0;//前进速度
	float set_spin_speed=0;//旋转速度
	float car_speed_max;//小车最大速度
	if(car_move_flag)//如果小车前进标记等于1
	{
		if(set_go_step_flag)//如果小车走固定的距离
		{
			//设置距离PID
			Set_Go_Step_Pid_Calculate(set_go_step_distance);
			//得到小车前进速度
			car_speed_forward=set_go_step_pid_uk;
		}
		
		//如果是循迹标记
	  if(tracking_flag)
	  {
			//如果不是固定步长
			if(!set_go_step_flag) 
			{
				//小车速度，最大速度
				car_speed_max=set_car_speed_forward-S2(error)*0.02f;
		
				if(car_speed_max<50) car_speed_max=50;
				//如果小车速度没有达到最大值，那么就不断 加8
				if(car_speed_forward<car_speed_max) car_speed_forward+=8;
				else car_speed_forward=car_speed_max;
				//否则就是最大速度
			}
			//方向PID计算用z轴的角速度做误差
			Direction_Pid_Calculate(error,-z_angular_speed);
			//设置旋转角度
			set_spin_speed=-direction_pid_uk;
		}
		
		//如果是设置的旋转角度
		if(set_spin_step_flag)
		{
			//将yaw角度做旋转反馈
			Set_Spin_Step_Pid_Calculate(yaw);
			//将误差反馈回来
			set_spin_speed=-set_spin_step_pid_uk;
		}
		set_forward_speed=car_speed_forward;
		if((set_forward_speed>0 && tracking_flag) || !tracking_flag)
			Mecanum_Wheel_Motor_Calculate(set_forward_speed,0,set_spin_speed);
		//只有前进时循迹
		else Mecanum_Wheel_Motor_Calculate(set_forward_speed,0,0);
	}
	
	//如果小车不前进，那么就停止
	else 
	{
		Mecanum_Wheel_Motor_Calculate(0,0,0);
	}
}

//计算角速度,定时器5ms读取
void Calculate_Angular_Speed(void)
{
	x_angular_speed=(float)mpu_gyro_x/gyro_LSB-gyro_biso_x;
  y_angular_speed=(float)mpu_gyro_y/gyro_LSB-gyro_biso_y;
  z_angular_speed=(float)mpu_gyro_z/gyro_LSB-gyro_biso_z;
}