光电组如何降低赛车速度？是改pwm还是通过编码器调节速度。

陈酿道光二五 · 发表于 2016-5-19 20:09:09

程序如下
#include "common.h"
#include "include.h"

uint8 yuzhi_max1;    //CCD1阈值上限
uint8 yuzhi_min1;    //CCD1阈值下限
uint8 yz;
uint8 Pixel1[131]; //CCD1数组(0~127像素数据、128min、129max、130阈值)
uint8 CCD_Flag = 0;
uint8 *a;             //CCD存储数组指针
int i;
int OK;
int16 mid=64;
int16 error=0,error1=0,error2=0;
extern uint8 WhiteNum;

int32 duoji_PWM=0;
int P1=35,D1=80;

uint16 set_speed=300,right_speed=0,left_speed=0;
int speed_error=0,speed_error_pre=0,speed_error_accum=0;
int P2=15,I2=0,D2=0;
int32 dianji_PWM;
uint8 read_times=0;

void PIT1_IRQHandler(void);
void portb_handler(void);
void data_init(void);
void Direction_first_line(void);
uint8 get_mid(uint8 a,uint8 b,uint8 c);
void duoji_PD();
void dianji_PID();
/*!
*  @BRIEF    main函数
*  @since    v5.0
*  @note    FTM PWM 测试
*/
void main(void)
{
data_init();
DisableInterrupts;
pit_init_ms(PIT1, 10);
set_vector_handler(PIT1_VECTORn ,PIT1_IRQHandler);    //设置PIT0的中断复位函数为 PIT0_IRQHandler

gpio_init(PTB9, GPI,1);

CCD_init();
LCD_Init();
//  FTM_port_mux(FTM2,FTM_CH1);
FTM_PWM_init(FTM2,FTM_CH1,50,7140);//6080极右  7545极左

//FTM_port_mux(FTM0,FTM_CH0);
   // FTM_port_mux(FTM0,FTM_CH1);
   // FTM_port_mux(FTM0,FTM_CH2);
   //    FTM_port_mux(FTM0,FTM_CH3);
FTM_PWM_init(FTM0,FTM_CH0,10000,0);
FTM_PWM_init(FTM0,FTM_CH1,10000,0);
FTM_PWM_init(FTM0,FTM_CH2,10000,0);
FTM_PWM_init(FTM0,FTM_CH3,10000,0);

lptmr_pulse_init(LPT0_ALT1,0xFFFF,LPT_Rising);
EnableInterrupts;
      enable_irq (PIT1_IRQn);
yuzhi();

OK=1;

while(1)
{
   LCD_CCD_SHOW(7,Pixel1);
   LCD_BL(2,0,yz);
LCD_BL(2,2,yuzhi_max1);
   LCD_BL(2,4,yuzhi_min1);
   LCD_BL(40,0,mid);
LCD_BL(40,2,WhiteNum);
LCD_BL(80,0,error+128);
LCD_BL(80,2,error1+128);

LCD_BL(80,4,right_speed);

if(!gpio_get (PTB9))
   while(1);

}
}

void data_init(void)
{
i=0;
OK=0;
yuzhi_max1=255;
yuzhi_min1=0;
yz = 0;
a=Pixel1;          //指针赋值
   for(i=0; i<131; i++)  //CCD存储数组清零初始化
   {
      *a++ = 0;
   }
}

void PIT1_IRQHandler(void)
{
  PIT_Flag_Clear(PIT1);
  CCD_Flag=1;
  if(OK==1)
  Get_Img();
  Direction_first_line();
duoji_PD();

read_times++;
if(read_times==2)
{

   right_speed=LPTMR0_CNR;
   LPTMR0_CNR=0;

   lptmr_pulse_init(LPT0_ALT2,0xFFFF,LPT_Rising);


}
else if(read_times==4)
{
read_times=0;
left_speed=LPTMR0_CNR;
LPTMR0_CNR=0;
lptmr_pulse_init(LPT0_ALT1,0xFFFF,LPT_Rising);
dianji_PID();

}


}

int LeftLine_pre=0,RightLine_pre=0;
uint8 LeftFlag_pre=0,RightFlag_pre=0;
void Direction_first_line(void)
{
   int32 i;
   int LineFlag=0;
   int LeftFlag=0;
   int RightFlag=0;
   int LeftLine=0;
   int RightLine=0;
   for(i=mid;i>2;i--)//
   {
         if((Pixel1[i+1]==1)&& (Pixel1==1) && (Pixel1[i-1]==0) && (Pixel1[i-2]==0)&& (Pixel1[i-3]==0) )
         {
            LeftLine=i;
            LeftFlag=1;
            break;
         }
   }
   for(i=mid;i<126;i++)
   {
      if( (Pixel1[i-1]==1)&&(Pixel1==1) && (Pixel1[i+1]==0) && (Pixel1[i+2]==0)&&(Pixel1[i+3]==0) )
         {
            RightLine=i;
            RightFlag=1;
            break;
         }
   }

   if(LeftFlag==1&&RightFlag==1)                         //两边都找到
   {
      mid = (LeftLine+RightLine)/2;
      error = 64 - mid;
   }

else if(RightFlag==0&&LeftFlag==1)                   //  右边丢线
   {  if(RightFlag_pre=1&&LeftFlag_pre==1)
      RightLine = LeftLine+(RightLine_pre-LeftLine_pre);
      else
         RightLine = LeftLine + 50;
      mid = (LeftLine+RightLine)/2;
      error = 64 - mid;

      if(LeftLine>110)
         error=error1;
   }
   else if(RightFlag==1&&LeftFlag==0)                   //
   {  if(RightFlag_pre=1&&LeftFlag_pre==1)
      LeftLine = RightLine-(RightLine_pre-LeftLine_pre);
      else
      LeftLine = RightLine - 50;
      mid = (LeftLine+RightLine)/2;
      error = 64 - mid;
         if(RightLine<18)
         error=error1;
   }
else                                     //
   {

               error = error1;
   }

      if(error>80)error=80;
      else if(error<-80)error=-80;
LeftFlag_pre=LeftFlag;
RightFlag_pre=RightFlag;
   LeftFlag=0;
   RightFlag=0;
   RightLine_pre=RightLine;
   LeftLine_pre=LeftLine;

   if(mid<=0) mid=0;
   if(mid>=127)mid=127;

  }
void Direction(void)
{

}

uint8 get_mid(uint8 a,uint8 b,uint8 c)
{
uint8 x;
if(a>b) {x=b;b=a;a=x;}
if(b>c) {x=c;c=b;b=x;}
if(a>b) {x=b;b=a;a=x;}
return b;
}
void duoji_PD()
{
if(error<10&&error>-10)
set_speed=650;
else if(error>=10&&error<20||error>-20&&error<=-10)
set_speed=500;
  else if(error>=20&&error<30||error>-30&&error<=-20)
set_speed=400;
else if(error>=30||error<=-30)
set_speed=350;


duoji_PWM=7140+P1*error+D1*(error-error1);
//error2=error1;
error1=error;

if(duoji_PWM<=6375)duoji_PWM=6375;//极右
else if(duoji_PWM>=7870)duoji_PWM=7870;//极左
FTM_PWM_Duty(FTM2,FTM_CH1,duoji_PWM);

/*duoji_PWM=13700+P1*error+D1*(error-error1);//13700
//error2=error1;
error1=error;

if(duoji_PWM<=12500)duoji_PWM=12500;//12500
else if(duoji_PWM>=15000)duoji_PWM=15000;//15000
FTM_PWM_Duty(FTM2,FTM_CH1,duoji_PWM);*/

}
void dianji_PID()
{
speed_error=set_speed-(right_speed+left_speed)/2;
//speed_error_accum+=speed_error;
dianji_PWM=P2*speed_error+/*I2*speed_error_accum*/+D2*(speed_error-speed_error_pre);
if(dianji_PWM>9900)dianji_PWM=9900;
else if(dianji_PWM<-9900)dianji_PWM=-9900;

if(dianji_PWM>=0)
{
   FTM_PWM_Duty(FTM0,FTM_CH0,dianji_PWM);//右正
   FTM_PWM_Duty(FTM0,FTM_CH1,0);//右反
   FTM_PWM_Duty(FTM0,FTM_CH2,dianji_PWM);
   FTM_PWM_Duty(FTM0,FTM_CH3,0);
}
else if(dianji_PWM<0)
{

   FTM_PWM_Duty(FTM0,FTM_CH0,0);//右正
   FTM_PWM_Duty(FTM0,FTM_CH1,-dianji_PWM);//右反
   FTM_PWM_Duty(FTM0,FTM_CH2,0);
   FTM_PWM_Duty(FTM0,FTM_CH3,-dianji_PWM);

}

}

ambition0123 · 发表于 2016-7-1 16:35:29

你好，能麻烦你讲一下程序吗？

ambition0123 · 发表于 2016-7-1 16:36:24

能不能慷慨解囊，来份程序啊

:'(:'(

极地霹雳 · 发表于 2016-7-5 20:50:40

通过编码器调节是什么意思？

holytak · 发表于 2016-7-5 23:56:19

當然是靠編碼器傳回來的直再利用PID調節

		自动登录	找回密码
密码			注册