📄 新建 文本文档.txt
字号:
get_offset();
get_avr();
}
void soft_init(void)
{
unsigned char i;
/* 初始化全局变量 */
vspeed=0;
flagA=LSPEED;
flagB=NONE;
flagBLIND=NONE;
zcount=2000;
cum_timer=0;
/* 机体初始化 */
fPWM(0); //正中
p=0;
do
{
refresh_vspeed();
getnewdata();
repeat_data();
}while (current.d_dep != NOERROR);
get_offset();
get_avr();
current.step=1;
arr[p]=current;
}
void PID_func(struct PID sPID) //通用算法，两个模块的共用算法
{
double rOut;
double rIn;
PIDInit ( &sPID );
sPID.Proportion = Kp; // Set PID Coefficients
sPID.Integral = Kintegral;
sPID.Derivative = Kderivative
sPID.SetPoint = Spoint; // Set PID Setpoint
rIn = pid_input ();
rOut = PIDCalc ( &sPID,rIn ); // Perform PID Interation
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
unsigned int d[8] , d_aver , out_f ; //存储探头数据,探头平均值
unsigned finder_d , finder_d2 ; //平均值差值
unsigned int speed , speed_b ; //储存小车速度
unsigned int d_state ; //储存探头状态
unsigned int s_cyc , cycle , suddcyc; //测速度用计数器
unsigned int keepspeed = 100 , speed_f ; //最高速度限制 , 脱离跑道标志
unsigned long distanse = 0 ; //记录路程
unsigned int distanse_s = 0 , distanse_f ;
unsigned int brake_cyc = 100 , brake_f ; //刹车
int black , black_temp , black_change ; //促存黑条位置,黑条标志
void delay (unsigned i)
{ while ( --i ) ;
}
void distanse_action ( void )
{
// if ( black < 7 && black > -7 )
distanse_s += black > 0 ? black : -black ;
// if ( black == 7 || black == -7 )
// distanse_s += 5 ;
if ( black_temp != black )
black_change = 1 ;
if ( black_temp == black )
black_change = 0 ;
black_temp = black ;
if ( distanse % 3 == 0 )
{
distanse_f = distanse_s ;
distanse_s = 0 ;
}
}
/*-----------电机动作-----------*/
void moto_action ( void )
{
if ( brake_cyc > 19 )
{
if ( distanse_f < 10 )
{
keepspeed += 2 ;
if ( black < 4 && black > -4 )
keepspeed += 2 ;
if ( black < 2 && black > -2 )
keepspeed += 2 ;
if ( keepspeed > 220 ) keepspeed = 220 ;
}
if ( distanse_f > 9 )
{
keepspeed -= 4 ;
if ( keepspeed < 80 ) keepspeed = 80 ;
FORWARD = 0 ;
// if ( black == 7 || black == -7 )
// {
// keepspeed = 100 ;
// }
}
if ( black_change == 1 )
{
keepspeed -= 35 ;
if ( keepspeed < 80 ) keepspeed = 80 ;
black_change == 0 ;
}
if ( speed < keepspeed )
if ( FORWARD < FULLPOWER ) //最高限速
FORWARD += 5 ;
if ( speed > keepspeed )
if ( FORWARD > LOWPOWER ) //最低限速
FORWARD = 120 ;
}
if ( distanse_f > 13 && speed > 110 ) //刹车
{
brake_cyc = 16 ;
if ( speed > 130 )
brake_cyc = 14 ;
if ( speed > 150 )
brake_cyc = 14 ;
}
if ( brake_cyc < 20 )
{
// brake_f = 1 ;
if ( brake_cyc %4 > 1 ) //防暴死急刹车
{
BELL = BELLON ;
FORWARD = 0 ;
RETURN = 205 ;
}
if ( brake_cyc %4 < 2 )
{
BELL = BELLON ;
FORWARD = 0 ;
RETURN = 0 ;
}
}
if ( brake_cyc > 19 )
{
BELL = BELLOFF ;
RETURN = 0 ;
// brake_f = 0 ;
}
}
/*-----------舵机动作-----------*/
void helm_action ( void )
{
/*31_19 小了向右转,大了向左转 */
switch ( black )
{
case -5 : PWMDTY7 = 30 ; break ;
case -4 : PWMDTY7 = 28 ; break ;
case -3 : PWMDTY7 = 27 ; break ;
case -2 : PWMDTY7 = 26 ; break ;
case -1 : PWMDTY7 = 25 ; break ;
case 0 : PWMDTY7 = 25 ; break ;
case 1 : PWMDTY7 = 25 ; break ;
case 2 : PWMDTY7 = 24 ; break ;
case 3 : PWMDTY7 = 23 ; break ;
case 4 : PWMDTY7 = 22 ; break ;
case 5 : PWMDTY7 = 20 ; break ;
}
}
/*-----------速度计算-----------*/
void getspeed ( void )
{ ulong p ;
p = TC0H ;
if ( s_cyc < 210 )
s_cyc ++ ;
if ( PA0H > 0 )
{
if ( s_cyc > 2 )
speed = 4000/s_cyc ;
s_cyc = 0 ;
distanse ++ ; //路程增加
distanse_action() ;
}
if ( s_cyc > 40 )
speed = 0 ;
if ( s_cyc > 40 )
distanse_f = 0 ;
if ( s_cyc > 40 )
black_change = 0 ;
}
/*---------计算黑条位置---------*/
void getd_state ( void )
{
unsigned int state_count = 0 ;
if ( d[3] < d_aver - finder_d ) //4探头
{
black = -1 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[1] < d_aver - finder_d ) //2探头_优先
{
black = -3 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[5] < d_aver - finder_d ) //6探头
{
black = 2 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[6] < d_aver - finder_d ) //7探头
{
black = 3 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[0] < d_aver - finder_d ) //1探头_优先
{
black = -4 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[2] < d_aver - finder_d ) //3探头_优先
{
black = -2 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[4] < d_aver - finder_d ) //5探头_优先
{
black = 1 ;
state_count ++ ;
d_state = 1 ;
}
if ( d[7] < d_aver - finder_d ) //8探头_优先
{
black = 4 ;
state_count ++ ;
d_state = 1 ;
}
if ( 4 == black )
{
out_f = 1 ;
} else
out_f = 0 ;
if ( 1 == out_f && 0 == state_count )
{
d_state = 0 ;
black = 5 ;
}
if ( -4 == black )
{
out_f = 1 ;
} else
out_f = 0 ;
if ( 1 == out_f && 0 == state_count )
{
d_state = 0 ;
black = -5 ;
}
}
/*--------获取探头数据---------*/
void getnewdata ( void )
{ unsigned char sta0, sta1;
unsigned int i ;
sta0 = ATD0STAT0;
sta1 = ATD0STAT1;
/*---------ATD0----------*/
if (ATD0STAT0 & ATD0STAT0_SCF_MASK)
{
d[0] = ATD0DR1;
d[1] = ATD0DR0;
d[2] = ATD0DR2;
d[3] = ATD0DR3;
d[4] = ATD0DR5;
d[5] = ATD0DR7;
d[6] = ATD0DR6;
d[7] = ATD0DR4;
}
/*------计算平均值--------*/
for ( i = 0 , d_aver = 0 ; i < 8 ; i++ )
d_aver += d[i] ;
d_aver = d_aver/8 ; //计算出平均值
finder_d = d_aver/2 ;
finder_d2 = d_aver/10 ;
}
void hardware_init()
{
/* 硬件初始化，包括启动定时器(放到最后一行)
* 此函数结束之后可以马上进入循环 */
/*--设置定时器寄存器--*/
MCCTL = 0xec ;
MCCNT = 0x5a00 ;
/*--------------------*/
/*--设置AD寄存器------*/
ATD0CTL2 = ATD0CTL2_ADPU_MASK;
ATD0CTL3 = 0x00; // 启用1通道转换队列
ATD0CTL5 = 0xB0; // 启动10位AD转换，数据右对齐，自动连续转换
ATD1CTL2 = ATD0CTL2_ADPU_MASK ;
ATD1CTL3 = 0x00; // 启用1通道转换队列
ATD1CTL5 = 0xB0; // 启动10位AD转换，数据右对齐，自动连续转换
/*--------------------*/
/*--------------------设置PWM寄存器------------------*/
/*--PWM7控制舵机,PWM5控制探LED,PWM0,PWM1控制驱动电机------*/
PWMPOL = 0x80 ; /*设置PWM7(舵机)极性*/
PWMCLK = 0xa0 ; /*PWM以SB为始终源,PWM5以SA为始终源,PWM4以A为始终源*/
PWMPRCLK = 0x72 ; /*设施始终源分频系数*/
PWMSCLA = 0x10 ; /*SA为A的三十二分之一倍*/
PWMSCLB = 0x05; /*SB为B的四十分之一倍*/
PWMPER5 = 0x40 ; /*设置红外LED频率*/
PWMDTY5 = 0x20 ;
PWMPER7 = 0xba; /*设置舵机PWM周期*/
PWMDTY7 = 12 ; /*舵机处在中间*/
PWMPER4 = 0xf0 ; /*设置电机PWM周期-为了自加时不溢出设为240*/
PWMPER0 = 0xf0 ; /*设置电机PWM周期-为了自加时不溢出设为240*/
PWMPER1 = 0xf0 ; /*设置电机PWM周期-为了自加时不溢出设为240*/
//PWMDTY4 = 80;
PWME = 0xb3; /*开启通道7,5,4 ,1,0*/
PWMDTY0 = 0 ;
PWMDTY1 = 180 ;
/*-------------------- */
/*---设置输入捕捉----- */
TCTL4 = 0xF6 ;
ICPAR = 0x0f ;
ICSYS = 0x02 ;
PACTL = 0x02 ;
/*--------------------*/
DDRJ = 0xff ; /*PORTJ口方向定义*/
DDRA = 0x81 ; /*PORTA口方向定义*/
DDRB = 0xfe ;
PORTB = 0xff ;
PUCR_PUPAE = 0x03 ;
PORTA_BIT0 = 1 ;
PORTA_BIT1 = 1 ;
}
/*------1毫秒中断-------*/
interrupt void test()
{ unsigned int i ;
// PORTB =~PORTB; /*用于程序运行指示*/
MCCNT = 0x5a00 ; /*定时器寄存器赋初值*/
MCFLG = 0x80 ; /*清楚中断标志位*/
cycle ++ ;
if ( brake_cyc < 100 )
brake_cyc ++ ; //刹车
getspeed () ;
getnewdata() ;
getd_state() ;
helm_action() ;
moto_action() ;
if ( PORTA_BIT1 == 1 )
{
if ( suddcyc < 2000 )
suddcyc ++ ;
if ( suddcyc < 1000 )
FORWARD = 0 ;
}
PORTB_BIT0 = 0 ;
if ( PORTB_BIT0 == 0 )
{ delay ( 1000 ) ;
if ( PORTB_BIT0 == 0 )
{
delay ( 1000 ) ;
if ( PORTB_BIT0 == 0 )
{
FORWARD = 0 ; //电机不动
suddcyc = 0 ;
if(cycle > 300 )
{
for ( i = 0 ; i < 8 ;i++ )
printp("%D " , d[i] ) ;
printp("speed:%5D" ,speed ) ;
printp("keepspeed:%d" ,keepspeed ) ;
printp("d_state:%d black:%d" , d_state , black ) ;
printp("FORWARD:%d\n" , FORWARD ) ;
cycle = 0 ;
}
}
}
}
}
void main(void)
{
unsigned char i;
soft_init();
EnableInterrupts; /*总中断开启*/
hardware_init();
printp (" start\n ") ;
while (1)
{
control_Check();
parameter_Modify();
#ifdef DESK_DEBUG
if (current.d_dep==0)
{
}
else
{ printp("W");
}
for (i=0;i<8;i++) printp("%d ",current.d[i]);
printp("%d%d%d%d[%d]%d%d\n",current.avr,current.vspeed,p,zcount,current.l,current.vspeed , arr[(p+1)%2].vspeed);
#endif
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -