/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修改Movement的值，比如，改成-60和60就比较慢了
入口参数：左轮编码器、右轮编码器
返回 值：速度控制PWM
作 者：平衡小车之家
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral;
float kp=80,ki=0.4;
//=============遥控前进后退部分=======================//
if(1==Flag_Qian) Movement=90/Flag_sudu; //===如果前进标志位置1 位移为负
else if(1==Flag_Hou) Movement=-90/Flag_sudu; //===如果后退标志位置1 位移为正
else Movement=0;
//=============速度PI控制器=======================//
Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.7; //===一阶低通滤波器
Encoder += Encoder_Least*0.3; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
if(Encoder_Integral>10000) Encoder_Integral=10000; //===积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===积分限幅
Velocity=Encoder*kp+Encoder_Integral*ki; //===速度控制
if(Turn_Off(Angle_Balance,Voltage)==1) Encoder_Integral=0; //===电机关闭后清除积分
return Velocity;
}
int main(void){ volatile unsigned long delay;
int i,j,state,intensity,counter;
SysTick_Init();
LED_Init();
counter = 0;
state = 1;
Alphabet('T');
Alphabet('E');
Alphabet('X');
Alphabet('A');
Alphabet('S');
for (i =0;i < 25;i++){
SpinUT();
}
while(1){
index = 0;
ADC_InitTimer0ATriggerSeq3(0, 0, CLOCKFREQ/SAMPLEFREQ);
while(index < MAXBUFFERSIZE){};
for(i=1; i<MAXBUFFERSIZE; i=i+1){
Turn_Off();
if(ADCbuffer[i] > 8){
intensity = Find_ADCindex(ADCbuffer[i]);
switch (state){
case 0://welcome
Animation0(Tdata[intensity]-1000);
if(counter > 200){
state = 1;
counter = 0;
}else{counter = counter +1;}
break;
case 1:
Animation1(intensity);
state = 1;
if(counter > 200){
state = 2;
counter = 0;
}else{counter = counter +1;}
break;
case 2:
Animation4(intensity);
if(counter > 200){
state = 3;
counter = 0;
}else{counter = counter +1;}
break;
case 3:
Animation3(intensity);
state = 3;
if(counter > 200){
state = 4;
counter = 0;
}else{counter = counter +1;}
break;
case 4:
RainDrop(Tdata[intensity]);
if(counter > 200){
state = 5;
counter = 0;
}else{counter = counter +1;}
break;
case 5:
Animation2(intensity);
if(counter > 200){
state = 6;
counter = 0;
}else{counter = counter +1;}
break;
case 6:
Animation5(intensity);
if(counter > 200){
state = 0;
counter = 0;
}else{counter = counter +1;}
break;
default:
Random();
break;
}
}
}
}
}
