智能车制作

标题: 调节速度P和I会对车子有影响，但车还是会往一个方向走，求大神帮忙， [打印本页]

作者: justlive 时间: 2015-6-1 16:23
标题: 调节速度P和I会对车子有影响，但车还是会往一个方向走，求大神帮忙，
/********************************************/
//函数 : 数度控制
//名称 : SpeedControl();
//
/*********************************************/
void SpeedControl(void)
{
   float fP;
   float fI;
   int fDelta1=0;
   int fDelta;
  // CarSpeed=(Left+Right)/2;
   CarSpeed = Left;
   Left=Right=0;
  // CarSpeed*=2;
   fDelta=0;

fDelta=fDelta1-CarSpeed;
   SPEED_P =0.5;
   fP=fDelta*SPEED_P;
   SPEED_I=0;
   fI=fDelta*SPEED_I;
   Speedlast+=fI;
   if(Speedlast<-50){
      Speedlast=-50;
   }
   if(Speedlast>50){
      Speedlast=50;
   }
   SpeedOld=SpeedNew;
   SpeedNew=fP+Speedlast;
   LCD_write_english_string(0,0,"SO");
   LCD_Write_Num(20,0,SpeedOld,5);
   LCD_write_english_string(0,4,"SN");
   LCD_Write_Num(20,4,SpeedNew,5);
}
/*****************************************/
//函数 : 速度平滑输出
//名称 : Speedput(void);
//
/******************************************/
void Speedput(void)
{
   float fValue;
   fValue=SpeedNew-SpeedOld;
   SpeedOut=fValue*(SpeedPeriod+1)/100+SpeedOld;
   LCD_write_english_string(0,2,"SO");
   LCD_Write_Num(20,2,SpeedOut,5);
}
/**********************************************/
//函数 : 电机驱动函数
//名称 : void SetMotorVoltage(float fLeftVoltage,float fRightVoltage)
//
/**********************************************/
void SetMotorVoltage(float fLeftVoltage,float fRightVoltage)
{
if(fLeftVoltage>0){
   if(fLeftVoltage>255){fLeftVoltage=255;}
   PWMDTY67=(int)fLeftVoltage;
   PWMDTY45=0;
}
else if(fLeftVoltage<0){
   fLeftVoltage=-fLeftVoltage;
   if(fLeftVoltage>255){fLeftVoltage=255;}
   PWMDTY67=0;
   PWMDTY45=(int)fLeftVoltage;
   }
  if(fRightVoltage>0){
   if(fRightVoltage>255){fRightVoltage=255;}
   PWMDTY23=(int)fRightVoltage;
   PWMDTY01=0;
   }
else if(fRightVoltage<0){
   fRightVoltage=-fRightVoltage;
   if(fRightVoltage>255){fRightVoltage=255;}
   PWMDTY23=0;
   PWMDTY01=(int)fRightVoltage;
}
}
/**********************************************/
// 函数 : 电机输出函数
// 名称 : Mtorput();
//
/**********************************************/
void Motorput(void)
{
   float fLeft,fRight;
   if(AngleOut>0){
      fLeft=AngleOut+SpeedOut/*-DirectionOut*/;
      fRight=AngleOut+SpeedOut/*+DirectionOut*/;
            }
if(AngleOut<0){
      fLeft=AngleOut-SpeedOut/*-DirectionOut*/;
      fRight=AngleOut-SpeedOut/*+DirectionOut*/;
            }
LeftOut=fLeft;
RightOut=fRight;
SetMotorVoltage(LeftOut,RightOut);
LCD_write_english_string(0,1,"LO");
LCD_Write_Num(20,1,LeftOut,5);
  }
/***********************************************/
// 函数 : 主函数
// 名称 : main();
//
/***********************************************/
void main(void)
{
   DisableInterrupts;
   PLL_INIT();
   init_pwm();
//  PORT_INIT();
   ATD_INIT();
   PIT_INIT();
   PACNT_INIT();
   PORT_Init();
   LCD_clear();
   LCD_init();
   EnableInterrupts;
   for(;;)
   {

   }
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt VectorNumber_Vpit0 void PIT_ISR(void)
{
PITTF_PTF0=1;
SpeedPeriod++;
Speedput();
// DirectionPeriod++;
// Directionput();
count = count+1;
if(count==1){    //脉冲计数
   TIM1_init();
   //  LeftPulse=PACNT;
      LCD_write_english_string(0,5,"LP");
   LCD_Write_Num(20,5,LeftPulse,5);
}
else if(count==2){  //AD采集

   // TIM2_init();
   ATD_capture();
}
else if(count==3){ //角度计算与直立
// RightPulse=PACNT;
   AngleCalculate();
   AngleControl();
   Motorput();
}
else if(count==4){ //速度控制
      SpeedCount++;
      Left+=LeftPulse;
   // Right+=RightPulse;
      Right+=LeftPulse;
      if(SpeedCount>=10){
         SpeedControl();
         SpeedCount=0;
         SpeedPeriod=0;
      }
}
else if(count==5){

      count=0;
}
}
#pragma CODE_SEG DEFAULT

欢迎光临智能车制作 (http://dns.znczz.com/)