📄 resa.c
字号:
case 0x06:switch(lock_direct){
case 0: case-1:case -2:sensor_e=Given-36;lock_direct=-1;break;
case 2:case 3:case 4:sensor_e=Given+70;lock_direct=3;break;
default:break;}break; /* 00000110 */
case 0x07:
case 0x02:switch(lock_direct){
case 0: case-1: case -2:sensor_e=Given-41;lock_direct=-1;break;
case 2:case 3:case 4:sensor_e=Given+70;lock_direct=3;break;
default:break;}break; /* 00000010 */
case 0x03:switch(lock_direct){
case 0:case-1:case -2:sensor_e=Given-53;lock_direct=-1;break;
case 2:case 3:case 4:sensor_e=Given+70;lock_direct=3;break;
default:break;}break; /* 00000011 */
case 0x01:switch(lock_direct){
case 0:case-1:case -2:sensor_e=Given-60;lock_direct=-1;break;
case 2:case 3:case 4:sensor_e=Given+70;lock_direct=3;break;
default:break;}break; /* 00000001 */
//偏右
case 0x38:
case 0x10:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+5;lock_direct=1;break;
case 4:case 5:sensor_e=Given+70;lock_direct=5;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 00010000 */
case 0x30:switch(lock_direct){
case 0: case 1: case 2:sensor_e=Given+17;lock_direct=1;break;
case 4: case 5:sensor_e=Given+70;lock_direct=5;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 00110000 */
case 0x70:
case 0x20:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+22;lock_direct=1;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 00100000 */
case 0x60:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+36;lock_direct=1;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 01100000 */
case 0xe0:
case 0x40:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+41;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 01000000 */
case 0xc0:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+53;lock_direct=1;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 11000000 */
case 0x80:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+60;lock_direct=1;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 10000000 */
/*纸质状态*/
case 0xc1:switch(lock_direct){
case 0:case 1:case 2:sensor_e=Given+53;lock_direct=1;break;
case -2:case -3:case -4:sensor_e=Given-70;lock_direct=-3;break;
default:break;}break; /* 11000001 */
case 0x83:switch(lock_direct){
case 0:case-1:case -2:sensor_e=Given-53;lock_direct=-1;break;
case 2:case 3:case 4:sensor_e=Given+70;lock_direct=3;break;
default:break;}break; /* 10000011 */
//在直角转弯状态已经做过处理
//case 0xf0: /* 11110000 */
//case 0xf8: /* 11111000 */
//case 0xfc: /* 11111100 */
//case 0x0f: /* 00001111 */
//case 0x1f: /* 00011111 */
//case 0x3f: /* 00111111 */
/*直角转弯检测到全白*/
case 0xff: /* 11111111 */
case 0x7e: /* 01111110 */
case 0x3c: /* 00111100 */
case 0x3e: /* 00111110 */
case 0x7c: /* 01111100 */
status_cnt2++;
if(status_cnt2>1){ //在规定时间内检测到3次以上全白则进入直角准备
status=2;
status_cnt2=0;
cnt2=0; //计时器复位
}
else if(cnt2>10){ //在检测全白次数不足4次时超过规定时间，则重新计数
status_cnt2=0;
cnt2=0;
}
break;
/*直角转弯检测左转*/
case 0xf0:/* 11110000 */
case 0xf8:/* 11111000 */
case 0xfc:/* 11111100 */
switch(lock_direct){
case -1:case 0:case 1:
status_cnt3++;
if(status_cnt3>1){
status=3;
status_cnt3=0;
cnt2=0;
}
else if(cnt2>10){
status_cnt3=0;
cnt2=0;
}
lock_direct=0;break;
case -2:case -3:case -4:
sensor_e=Given-70;lock_direct=-3;break;
default:break;}break;
/*直角转弯检测右转*/
case 0x0f:/* 00001111 */
case 0x1f:/* 00011111 */
case 0x3f:/* 00111111 */
switch(lock_direct){
case -1:case 0:case 1:
status_cnt4++;
if(status_cnt4>1){
status=4;
status_cnt4=0;
cnt2=0;
}
else if(cnt2>10){
status_cnt4=0;
cnt2=0;
}
lock_direct=0;break;
case 2:case 3:case 4:
sensor_e=Given+70;lock_direct=3;break;
default:break;}break;
default:
break;
}
sensor_sum+=sensor_e;
sensor_sum-=sensor_ee[sensor_p];
sensor_ee[sensor_p]=sensor_e;
sensor_p++;
sensor_p&=3;
sensor_avg=sensor_sum>>2;
sensor_c=sensor_avg;
/*微分保持器,大于10时保持续100ms*/
sensor_D=sensor_avg-sensor_E;
if(sensor_D>2||sensor_D<-2){
sensor_D_cnt=500;
sensor_DD=sensor_D;
}
if(sensor_D_cnt<0){
sensor_DD=sensor_D;
sensor_D_cnt=0;
}
sensor_D_cnt--;
/*带反向快速补偿的条件积分*/
if(sensor_cc<-50)KI_left=1;
if(sensor_cc>10)KI_left=0;
if(sensor_cc>50)KI_right=1;
if(sensor_cc<-10)KI_right=0;
if(sensor_c>0&&sensor_D>0){
sensor_cc+=sensor_D;
if(KI_left==1){
sensor_cc+=sensor_D;
sensor_cc+=sensor_D;}
}
if(sensor_c<0&&sensor_D<0){
sensor_cc+=sensor_D;
if(KI_right==1){
sensor_cc+=sensor_D;
sensor_cc+=sensor_D;}
}
switch (sensor_avg/10){
case 0:
case 1:
case -1:
handle_angle=-(sensor_c*40+sensor_cc*22+sensor_DD*500)/10;
break;
case 2:
case -2:
case 3:
case -3:
handle_angle=-(sensor_c*90+sensor_cc*22+sensor_DD*500)/10;
break;
case 4:
case -4:
case 5:
case -5:
case 6:
case -6:
default:
handle_angle=-(sensor_c*90+sensor_cc*22+sensor_DD*700)/10;
break;
}
// handle_angle=-(sensor_c*KP+sensor_cc*KI+sensor_DD*KD)/10;
if(sensor_D_cnt1>2)sensor_D=0;
else sensor_D_cnt1=1;
sensor_E=sensor_avg;
}
/*右轮外部中断函数*/
#pragma interrupt( IRQ0 )
void IRQ0(void)
{ ISR &= 0xfe; /* Flag clear */
IRQ_right0++; IRQ_right1++;
real_speed_right=42400/IRQ_right_cnt0;
if(IRQ_right0&0x03);
else {cycle_speed_right=16964/(IRQ_right_cnt00/10);
IRQ_right_cnt00=0;
IRQ_right0=0;}
IRQ_right_cnt0=0;
}
/*左轮外部中断函数*/
#pragma interrupt( IRQ1)
void IRQ1(void)
{
ISR &= 0xfd; /* Flag clear */
IRQ_left0++; IRQ_left1++;
real_speed_left=42400/IRQ_left_cnt1;
if(IRQ_left0&0x03);
else {cycle_speed_left=16964/(IRQ_left_cnt11/10);
IRQ_left_cnt11=0;
IRQ_left0=0;}
IRQ_left_cnt1=0;
}
/************************************************************************/
/* Timer main unit */
/* Argument Timer value 1=1ms */
/************************************************************************/
void timer( unsigned long timer_set )
{
cnt0 = 0;
while( cnt0 < timer_set );
}
/************************************************************************/
/* Sensor state detection */
/* AArgument Mask value */
/* RReturn value Sensor value */
/************************************************************************/
unsigned char sensor_inp( unsigned char mask )
{
unsigned char sensor,ret,i;
sensor = P7DR;
/* Since bit0 is for left and bit7 is for right in the new sensor board which is reversed */
/* to the previous sensor board, the bit has been replaced to maintain compatibility. */
for( i=0; i<8; i++ ){
ret >>= 1; /* Right shift of return value */
ret |= sensor & 0x80; /* Ret bit7 = in bit7 */
sensor <<= 1; /* Left shift of argument */
}
ret &= mask;
return ret;
}
/************************************************************************/
/* Reading of DIP switch value */
/* Return value Switch value 0 - 15 */
/************************************************************************/
unsigned char dipsw_get( void )
{
unsigned char sw;
sw = ~P6DR;sw &= 0x0f; /* Reading of DIP switch */
return sw;
}
/************************************************************************/
/* Push switch value reading */
/* Return value Switch value ON: "1" and OFF: "0" */
/************************************************************************/
unsigned char pushsw_get( void )
{
unsigned char sw;
sw = ~PBDR;sw &= 0x01; /* Reading of port having a push switch */
return sw;
}
/************************************************************************/
/* LED control */
/* Argument Switch value (LED1,LED0)=(bit1,bit0) "0":OFF , "1":ON */
/* Example 0x3->(LED1,LED0)=(ON,ON) : 0x2->(LED1,LED0)=(ON,OFF) */
/************************************************************************/
void led_out( unsigned char led )
{
unsigned char data;
led = ~led;
led <<= 6;
data = PBDR & 0x3f;
PBDR = data | led;
}
/************************************************************************/
/* Speed Control */
/* Argument Left motor: -1000 - 1000 , Right motor: -1000 - 1000 */
/* 0:Stop,100:normal rotation 100%,-100:Reverse 100% */
/************************************************************************/
void speed( int accele_l, int accele_r )
{
unsigned char sw_data;
unsigned long speed_max;
// sw_data = dipsw_get() + 5; /* DIP switch read */
sw_data=sw_speed_g+17;//sw_data=sw_speed_g*2+14;
speed_max = (unsigned long)(PWM_CYCLE-1) * sw_data / 20;
/* Left motor */
if( accele_l >= 0 ) {
PBDR &= 0xfb;
ITU3_BRB = speed_max * accele_l / 1000;
} else {
PBDR |= 0x04;
accele_l = -accele_l;
ITU3_BRB = speed_max * accele_l / 1000;
}
/* Right motor */
if( accele_r >= 0 ) {
PBDR &= 0xf7;
ITU4_BRA = speed_max * accele_r / 1000;
} else {
PBDR |= 0x08;
accele_r = -accele_r;
ITU4_BRA = speed_max * accele_r / 1000;
}
}
/************************************************************************/
/* Servo steering operation */
/* Argument Servo operation angle: -90 ~ 90 */
/* -90~0 to the left turn , 0:straight ,0~90 to the right turn */
/************************************************************************/
void handle( int angle )
{
ITU4_BRB = SERVO_CENTER - (angle * HANDLE_STEP)/10;
}
void handle_stop( int angle )
{
ITU2_GRB = SERVO_CENTER - (angle * HANDLE_STEP)/10;
}
/************************************************************************/
/* end of file */
/************************************************************************/
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -