#include
#include
#include "PWM.h"
#include "Bang.h"
#include "video.h"
#include "Image_process.h"
#define Kp 3
#define Kd 0
#define Speed_base 90 //////////////////////////////////////////////////////
// #define Speed_min 50
#define Angle_max 30
#define T_ks 3
#define Ksc 5
#define Ksd 6
#define Kss 7
// #define Line_Max 50
// #define Line_Min 10
unsigned char Line_Max,Speed_up,Speed_min,Speed_s;
signed char K_line,U=0; //中间变量
signed char Out_angle,Out_speed; //输出量
signed char K_line0=0;
signed char K_line1=0;
signed int Out_ang0=39,Out_ang;
unsigned char Line_middle=39,Y_X_max,Y_X_min,Y_max,Counter=T_ks,D=0,S=0,C=0;
unsigned char Ks=5,Flag=0,M,N;
unsigned char Now_line ;
//*****************************************************************************************
void Out_sp_ang(unsigned char Imagepoint[])
{
unsigned char i,j=0,X_max=0,X_min=255;
signed int u;
if(Error==0)
{
Image_process(&Imagepoint[0]);
for(i=FirstLine;i<57;i++)
{
if(Imagepoint[i]==255)
break;
}
Y_max=i-1;
if(Y_max>9)
{
if(Now_Speed<=Speed_min-10)
Now_line=Line_Max+10;
else
Now_line=Line_Max+(Now_Speed-Speed_min)/3;
if(Imagepoint[Now_line]<128) //第Now_line行有黑点(若第i行无黑点，则Imagepoint[i]=255)
K_line=Imagepoint[Now_line]-Line_middle;
else
{
for(i=10;i
{
K_line=(Imagepoint[i]-Line_middle)*Now_line/i;
break;
}
else j=1;
}
}
/////////////////////////////////////速度////////////////////////////////////
if(Track==0)
{ Ks=Ksd;
if(Out_speed>Speed_base+Speed_up)
Out_speed-=10;
if(Out_speed
// if(Out_speed
// if(Out_speed>Speed_base+Speed_up)
// Out_speed=Speed_base+Speed_up;
//
// Out_angle=K_line*Kp/Ks;
}
if(Track==1)
{Ks=Kss;
if(Out_speed>Speed_s)
Out_speed-=10;
if(Out_speed
// Out_angle=K_line*Kp/Ks;
}
if(Track==2)
{ Ks=Ksc;
if(K_line>0)
{ u=Out_angle ;
if(Out_speed>Speed_base+Speed_up-(unsigned char)u*u/80)
Out_speed-=10;
else Out_speed+=6;
// if(Now_Speed>60)
// Out_angle=K_line*Kp/Ks+(Now_Speed-60)/3;
// else Out_angle=K_line*Kp/Ks ;
// else
// Out_speed=Speed_base-K_line;
}
else
{ u=-Out_angle ;
if(Out_speed>Speed_base+Speed_up-(unsigned char)u*u/80)
Out_speed-=10;
else Out_speed+=6;
// if(Now_Speed>60)
// Out_angle=K_line*Kp/Ks+(Now_Speed-Speed_min-1)/15;
// else Out_angle=K_line*Kp/Ks ;
// else
// Out_speed=Speed_base+K_line;
}
}
/////////////////////////////////////////舵机///////////////////////////////////
Out_angle=K_line*Kp/Ks;
// {
// Out_angle=K_line*Kp/Ks+Kd*(K_line+K_line1-2*K_line0);
// K_line1=K_line0;
// K_line0=K_line ;
// }
}
if(Y_max>32&&Y_max<40)
{
if(C
else C=T_ks;
D=0;
}
else if(Y_max>40&&Y_max<54)
{
if(D
else D=T_ks;
C=0;
}
else{C=0;D=0;}
if(C==T_ks)
{
if(Out_speed
else Out_speed-=5;
}
if(D==T_ks)
{
if(Out_speed
else Out_speed-=5;
}
if(j==1)
{ if(Out_speed>Speed_min)
Out_speed-=12;
else
Out_speed=Speed_min;
if(Out_angle>0)
Out_angle+=2;
if(Out_angle<=0)
Out_angle-=2;
}
///////////////////////////////////////出错////////////////////////////////////
}
else
{ if(Out_speed>Speed_min)
Out_speed-=12;
else
Out_speed=Speed_min;
if(Out_angle>0)
Out_angle+=2;
if(Out_angle<=0)
Out_angle-=2;
}
// PORTB=~Out_speed;
if(Now_Speed>Flag)
Flag=Now_Speed ;
// PORTB=~Out_angle ;
//PORTB=~Track;
PORTB=~Y_max ;
//PORTB=~(Error*0xff);
}