平衡小车pid调参

直立环（kp和kd）

1.确定机械中值：把小车放地上，确定能让小车保持平衡的角度（俯仰角）

2.确定kp的极性（令kd=0），pwm最大值减去死区剩下的即为输出的最大pwm大小，所以kp*bias<=输出的最大pwm大小，bias的角度设为最大速度时的倾斜角度，然后可以得出kp的最大值，然后从kp的中间开始往左右调参，确定最佳kp值，极性确定即当前kp值使用时如果小车轮子前进的方向与小车倾倒的方向一致即极性正确

3.kd确定也同上差不多（令kp=0）gyro*kd

在直立环中，PID的入口参数为：平衡小车的姿态角和姿态角对应的角速度。

加入D控制，微分项，相当于一个阻尼力：在偏差项P占主导时，微分项D的影响相对来说非常小，然而当偏差趋于零时，微分项的作用就是，减慢受控物理量的变化速率。

/**************************************************************************************************************
*函数名:Vertical_Ring_PD()
*功能:直立环PD控制
*形参:(float Angle):x轴的角度/(float Gyro):x轴的角速度
*返回值:经过PID转换之后的PWM值
**************************************************************************************************************/
//直立环的PD

Balance_PWm =Vertical_Ring_PD(outMpu.pitch,outMpu.gyro_y);//直立环
int	Vertical_Ring_PD(float Angle,float Gyro)
{
	 float Bias;//目标偏差
	 int balance;
   Bias=Angle-Mechanical_balance;//目标值减去机械中值（不一定为0）
   balance=PID.Balance_Kp*Bias+ Gyro*PID.Balance_Kd;
	
	return balance;
		
	 //printf("balance = %f\n",balance);
}

速度环（kp和ki）

ki取kp的200分之1

速度环过大会导致直立时摇晃，过小则无法在受到外力时保持平衡

确定kp的正负需要将小车拿起来转到其中一个轮子，如果另一个轮子转动方向与其一致，则当前kp的极性正确

目标值是放下小车时刻，编码器的读数就是小车（车轮）的速度，前一次获得的速度与当前获得的速度之差就是微分速度变化率，即加速度

每隔一定周期，我们获取一次编码器的速度，然后与用户期望的速度，得到一个偏差，这个偏差就是P（比例）

/**************************************************************************************************************
*函数名:Vertical_speed_PI()
*功能；速度环PI控制
*形参:(int encoder_left):左轮编码器值/(int encoder_right):编码器右轮的值/(float Angle):x轴角度值
*返回值:
**************************************************************************************************************/
Velocity_Pwm = Vertical_speed_PI(Encoder_left,Encoder_right,outMpu.pitch,Movement );//速度环
int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement )
{
	static float Velocity,Encoder_Least,Encoder;
	static float Encoder_Integral;
	Encoder_Least =(encoder_left+encoder_right)-0;    //获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
	Encoder *= 0.8f;																	//一阶低通滤波器 ，上次的速度占85%
	Encoder += Encoder_Least*0.2f;                   //一阶低通滤波器， 本次的速度占20% 
	Encoder_Integral +=Encoder;                       //积分出位移 积分时间：10ms
	Encoder_Integral=Encoder_Integral-Movement; 
	//if(Movement == 0 ) Encoder_Integral=0;
	if(Encoder_Integral>10000)  	Encoder_Integral=10000;           //积分限幅
	if(Encoder_Integral<-10000)	  Encoder_Integral=-10000;            //积分限幅

	Velocity=Encoder*PID.Velocity_Kp+Encoder_Integral*PID.Velocity_Ki;      //速度控制
	
	
	if(Turn_off(Angle)==1)   Encoder_Integral=0;            //电机关闭后清除积分
	return Velocity;
}

转向环（kp和kd）

/**************************************************************************************************************
*函数名:Vertical_turn_PD()
*功能:转向环PD
*形参:无  CCD小于64左转、CCD大于64右转。 yaw = z轴陀螺仪数值
*返回值:无
***************************************************************************************************************/
Turn_Pwm = Vertical_turn_PD(Contrl_Turn,outMpu.gyro_z);
int Vertical_turn_PD(u8 CCD,short yaw)
{
		float Turn;     
    float Bias;	  
	  Bias=CCD-64;
	  Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
	  return Turn;
}

总

/**************************************************************************************************************
*函数名:Car_Task_100HZ(void)
*功能；100hz任务
*形参:无
*返回值:无
*确定直立环、速度环、转向环以及设置电机pwm
**************************************************************************************************************/
void Car_Task_100HZ(void)
{
	//HC_SRC04_Start();
	Encoder_left = Read_Encoder(1);
	Encoder_right = -Read_Encoder(2);//读编码器
	Balance_PWm =Vertical_Ring_PD(outMpu.pitch,outMpu.gyro_y);//直立环
	Movement = BT_Data.y/2.4;
	Contrl_Turn = BT_Data.x/2+64;
	Velocity_Pwm = Vertical_speed_PI(Encoder_left,Encoder_right,outMpu.pitch,Movement );//速度环
	Turn_Pwm = Vertical_turn_PD(Contrl_Turn,outMpu.gyro_z);
	Motor1 = Balance_PWm+Velocity_Pwm+Turn_Pwm ;
	Motor2 = Balance_PWm+Velocity_Pwm-Turn_Pwm ;
//	Motor1 = Velocity_Pwm;
//	Motor2 = Velocity_Pwm;
	PWM_Limiting(&Motor1,&Motor2);//pwm限幅
	if(Turn_off(outMpu.pitch)==1){//跌落保护
		Motor1 = 0;
		Motor2 = 0; 
	}
	Set_PWM(Motor1,Motor2);
}

/**************************************************************************************************************
*函数名:Set_PWM()
*功能:输出PWM控制电机
*形参；(int motor1):电机1对应的PWM值/(int motor2):电机2对应的PWM值
*返回值:无
*************************************************************************************************************/


//void Set_PWM(int motor1,int motor2)
//{
//	if(motor1<0)	{
//		AIN2(0);
//		PWMA=Dead_Zone+(abs(motor1))*1.17;
//	}
//	else 	{
//		AIN2(1);
//	PWMA=7999-Dead_Zone-(abs(motor1))*1.17;
//	}
//	if(motor2>0)	{
//		BIN2(1);
//		PWMB=7999-Dead_Zone-(abs(motor2))*1.17;	
//	}
//	else { 
//		BIN2(0);
//		PWMB=Dead_Zone+(abs(motor2))*1.17;
//	}
//	printf("PWMA = %d\n",PWMA);
//	printf("PWMB = %d\n",7999-PWMB);
//}


void Set_PWM(int motor1,int motor2)
{
	if(motor1>0)
{			
	PWMA2=Dead_Zone+(abs(motor1))*1.17;
	PWMA1=0;
    
	}
	else 	
{ 
	PWMA2 =0;
	PWMA1=Dead_Zone+(abs(motor1))*1.17;
	}
	
	
	if(motor2>0)		{	
		
		PWMB2 = Dead_Zone+(abs(motor2))*1.17;
		PWMB1 = 0;
		}
	else 
	{	 		PWMB2 = 0;
	PWMB1=Dead_Zone+(abs(motor2))*1.17;
	}
}