倒立摆基本代码

小车平衡之家：

硬件：用到ADC输出的角度传感器 和 带编码器的直流电机

5ms 进入一次定时器中断

//1：读取编码器的值
Encoder=Read_Encoder(1);
             	        
//2：获得角度传感器的ADC平均值 + 角度PD控制	
Angle_Balance=Get_Adc_Average(7,15);                  
Balance_Pwm=balance(Angle_Balance);    
                              
//3: 变成25ms进入一次位置控制
if(++Position_Target>4)
{
  Position_Pwm=Position(Encoder),Position_Target=0;
}

//4：最终的电机pwm融合
    Moto=Balance_Pwm-Position_Pwm;  

//5 ：限幅+赋PWM寄存器
.......................                        

ADC求平均值

u16 Get_Adc_Average(u8 ch,u8 times)
{
	u32 temp_val=0;
	u8 t;
	for(t=0;t<times;t++)
	{
		temp_val+=Get_Adc(ch);
		delay_us(200);
	}
	return temp_val/times;
} 

角度的PD控制

int balance(float Angle)
{  
 	float Bias;                       //倾角偏差
	int balance;                      //PWM返回值 
	static float Last_Bias,D_Bias;    //PID相关变量
	
	//偏差
	Bias=Angle-ZHONGZHI;     //= 实际 - 目标
	D_Bias=Bias-Last_Bias;   //= 这次的偏差 - 上次的偏差         
	Last_Bias=Bias;          //记录上一次的偏差
	
	//计算角度PID的输出
	balance=-Balance_KP*Bias-D_Bias*Balance_KD;   
	
	 
	return balance;
}

编码器电机位置的PD控制

int Position(int Encoder)
{  
  static float Last_Position,Position_Bias,Position_Differential;
  static float Position_Least;
  static float Position_PWM ;
  
  //位置的偏差（获得这次的误差）
  //一阶低通滤波器 （Kp的乘数=这次的0.2+上次的0.8） 
  Position_Least =Encoder-Position_Zero;//这次的误差         
  Position_Bias *=0.8;		   
  Position_Bias += Position_Least*0.2;	
  
  // Kd的乘数=这次的误差-上次的误差          
  Position_Differential=Position_Bias-Last_Position;
  Last_Position=Position_Bias;//保存这次误差
  
  Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD; //===速度控制	
	  
	  return Position_PWM;
}

Balance_KP=50,Balance_KD=244,Position_KP=25,Position_KD=600;