智能车制作
标题:
光电组如何降低赛车速度？是改pwm还是通过编码器调节速度。
[打印本页]
作者:
陈酿道光二五
时间:
2016-5-19 20:09
标题:
光电组如何降低赛车速度？是改pwm还是通过编码器调节速度。
程序如下
#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
你好，能麻烦你讲一下程序吗？
作者:
ambition0123
时间:
2016-7-1 16:36
能不能慷慨解囊，来份程序啊
:'(:'(
作者:
极地霹雳
时间:
2016-7-5 20:50
通过编码器调节是什么意思？
作者:
holytak
时间:
2016-7-5 23:56
當然是靠編碼器傳回來的直 再利用PID調節
欢迎光临 智能车制作 (http://dns.znczz.com/)
Powered by Discuz! X3.2
:'(:'(