?? kit06.c
字號:
pattern = 54;
cnt1 = 0;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
/* Center straight */
handle( 0 );
speed( 80 ,80 );
break;
case 0x04:
/* Move slightly to the left turn slightly to the right */
handle( 5 );
speed( 80 ,70 );
break;
case 0x06:
/* Move somewhat to the left turn somewhat to the right */
handle( 10 );
speed( 70 ,60 );
break;
case 0x07:
/* Move a medium amount to the left turn a medium amount to the right */
handle( 18 );
speed( 60 ,50 );
break;
case 0x03:
/* Move significantly to the left turn significantly to the right */
handle( 28 );
speed( 50,30 );
pattern = 12;
break;
case 0x20:
/* Move slightly to the right turn slightly to the left */
handle( -5 );
speed( 70,80 );
break;
case 0x60:
/* Move somewhat to the right turn somewhat to the left */
handle( -10 );
speed( 60 ,70 );
break;
case 0xe0:
/* Move a medium amount to the right turn a medium amount to the left */
handle( -18 );
speed( 50,60 );
break;
case 0xc0:
/* Move significantly to the right turn significantly to the left */
handle( -28 );
speed( 30 ,50 );
pattern = 13;
break;
default:
break;
}
break;
case 54:
/* Check for completion of the right lane change */
if( sensor_inp(MASK4_4) == 0x3c ||sensor_inp(MASK4_4) == 0x38 || sensor_inp(MASK4_4) == 0x1c
|| sensor_inp(MASK4_4) == 0x18|| sensor_inp(MASK4_4) == 0x30 || sensor_inp(MASK4_4) == 0x0c ) {
led_out( 0x0 );
pattern = 11;
cnt1 = 0;
}
break;
case 61:
/* Processing when the first left half line is detected */
if( check_crossline() ) { /* Also check the crossline during large turns. */
pattern = 21;
break;
}
led_out( 0x1 );
handle( 0 );
speed( 0 ,0 );
pattern = 62;
cnt1 = 0;
break;
case 62:
/* Read and ignore the second line. */
if( check_crossline() ) { /* Also check the crossline during large turns. */
pattern = 21;
break;
}
if( cnt1 > 80 ) {
pattern = 63;
cnt1 = 0;
}
break;
case 63:
/* Trace, lane change after a left half line */
if( sensor_inp(MASK4_4) == 0x00 ) {
handle( -15 ); //handle( -15 );
speed( 20,40 );
pattern = 64;
cnt1 = 0;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
/* Center straight */
handle( 0 );
speed( 70 ,70 );
break;
case 0x04:
/* Move slightly to the left turn slightly to the right */
handle( 5 );
speed( 70 ,68 );
break;
case 0x06:
/* Move somewhat to the left turn somewhat to the right */
handle( 10 );
speed( 70 ,60 );
break;
case 0x07:
/* Move a medium amount to the left turn a medium amount to the right */
handle( 18 );
speed( 50 ,40 );
break;
case 0x03:
/* Move significantly to the left turn significantly to the right */
handle( 28 );
speed( 45 ,30 );
pattern = 12;
break;
case 0x20:
/* Move slightly to the right turn slightly to the left */
handle( -5 );
speed( 68 ,70 );
break;
case 0x60:
/* Move somewhat to the right turn somewhat to the left */
handle( -10 );
speed( 60 ,70 );
break;
case 0xe0:
/* Move a medium amount to the right turn a medium amount to the left */
handle( -18 );
speed( 40 ,50 );
break;
case 0xc0:
/* Move significantly to the right turn significantly to the left */
handle( -28 );
speed( 30 ,50 );
pattern = 13;
break;
default:
break;
}
break;
case 64:
/* Check for completion of the left lane change */
if( sensor_inp(MASK4_4) == 0x3c ||sensor_inp(MASK4_4) == 0x38 || sensor_inp(MASK4_4) == 0x1c
|| sensor_inp(MASK4_4) == 0x18|| sensor_inp(MASK4_4) == 0x30 || sensor_inp(MASK4_4) == 0x0c ) {
led_out( 0x0 );
pattern = 11;
cnt1 = 0;
}
break;
default:
/* Return to the standby state for cases that were none of the above. */
pattern = 0;
break;
}
}
}
/************************************************************************/
/* H8/3048F-ONE built-in peripheral function initialization */
/************************************************************************/
void init( void )
{
/* I/O port input/output settings */
P1DDR = 0xff;
P2DDR = 0xff;
P3DDR = 0xff;
P4DDR = 0xff;
P5DDR = 0xff;
P6DDR = 0xf0; /* DIP switch on the CPU board */
P8DDR = 0xff;
P9DDR = 0xf7; /* Communication port */
PADDR = 0xf7; /* Start bar detection sensor */
PBDR = 0xc0;
PBDDR = 0xfe; /* Motor driver board Vol. 3 */
/* Since the sensor board P7 is input only, no input/output settings are required. */
/* ITU0: interrupt every 1 ms */
ITU0_TCR = 0x23;
ITU0_GRA = TIMER_CYCLE;
ITU0_IER = 0x01;
/* ITU3,4: reset synchronized PWM mode for left/right motors and servo */
ITU3_TCR = 0x23;
ITU_FCR = 0x3e;
ITU3_GRA = PWM_CYCLE; /* Period setting */
ITU3_GRB = ITU3_BRB = 0; /* Left motor PWM setting */
ITU4_GRA = ITU4_BRA = 0; /* Right motor PWM setting */
ITU4_GRB = ITU4_BRB = SERVO_CENTER; /* Servo PWM setting */
ITU_TOER = 0x38;
/* ITU counter start */
ITU_STR = 0x09;
}
/************************************************************************/
/* ITU0: Interrupt handling */
/************************************************************************/
#pragma interrupt( interrupt_timer0 )
void interrupt_timer0( void )
{
ITU0_TSR &= 0xfe; /* Clear the flag */
cnt0++;
cnt1++;
}
/************************************************************************/
/* Timer main unit */
/* Argument: timer value; 1 = 1 ms */
/************************************************************************/
void timer( unsigned long timer_set )
{
cnt0 = 0;
while( cnt0 < timer_set );
}
/************************************************************************/
/* Sensor state detection */
/* Argument: Mask value */
/* Return value: sensor value */
/************************************************************************/
unsigned char sensor_inp( unsigned char mask )
{
unsigned char sensor;
sensor = P7DR;
/* Since the new sensor board has bit 0 on the left and bit 7 on the right when facing the board, which is */
/* the reverse of the previous sensor board, the bit order must be reversed to maintain compatibility. */
sensor = bit_change( sensor ); /* Reverses the bit order */
sensor &= mask;
return sensor;
}
/************************************************************************/
/* Crossline detection processing */
/* Return value - 0: No crossline, 1: Crossline detected */
/************************************************************************/
int check_crossline( void )
{
unsigned char b;
int ret;
ret = 0;
b = sensor_inp(MASK2_2);
if( b==0x66 || b==0x64 || b==0x26 || b==0x62 || b==0x46 ) {
ret = 1;
}
return ret;
}
/************************************************************************/
/* Right half line detection processing */
/* Return value - 0: No half line, 1: Half line detected */
/************************************************************************/
int check_rightline( void )
{
unsigned char b;
int ret;
ret = 0;
b = sensor_inp(0xff);
if( b==0x0f || b==0x1f ) {
ret = 1;
}
return ret;
}
/************************************************************************/
/* Left half line detection processing */
/* Return value - 0: No half line, 1: Half line detected */
/************************************************************************/
int check_leftline( void )
{
unsigned char b;
int ret;
ret = 0;
b = sensor_inp(0xff);
if( b==0xf0 || b==0xf8 ) {
ret = 1;
}
return ret;
}
/************************************************************************/
/* DIP switch value readout */
/* Return value: Switch values 0 to 15 */
/************************************************************************/
unsigned char dipsw_get( void )
{
unsigned char sw;
sw = ~P6DR; /* Reads out the DIP switch. */
sw &= 0x0f;
return sw;
}
/************************************************************************/
/* Push switch value readout */
/* Return value: Switch value - On: 1, Off: 0 */
/************************************************************************/
unsigned char pushsw_get( void )
{
unsigned char sw;
sw = ~PBDR; /* Reads the port that has the switch. */
sw &= 0x01;
return sw;
}
/************************************************************************/
/* Start bard detection sensor read function */
/* Return value: 1: Sensor on (bar present), 0: sensor off (bar open) */
/************************************************************************/
unsigned char startbar_get( void )
{
unsigned char b;
b = ~PADR; /* Start bar signal read */
b &= 0x08;
b >>= 3;
return b;
}
/************************************************************************/
/* LED control */
/* Argument: LED switching values, LED0: bit 0, LED 1: bit 1; 0: Off, 1: On */
/* Examples: 0x3 LED1: on, LED0: on, 0x2: LED1: on, LED0: off */
/************************************************************************/
void led_out( unsigned char led )
{
unsigned char data;
led = ~led;
led <<= 6;
data = PBDR & 0x3f;
PBDR = data | led;
}
/************************************************************************/
/* Speed control */
/* Arguments - Left motor: -100 to 100, right motor: -100 to 100 */
/* 0: stopped, 100: forward 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; /* Reads out the DIP switch. */
speed_max = (unsigned long)(PWM_CYCLE-1) * sw_data / 20;
/* Left motor */
if( accele_l >= 0 ) {
PBDR &= 0xfb;
ITU3_BRB = speed_max * accele_l / 100;
} else {
PBDR |= 0x04;
accele_l = -accele_l;
ITU3_BRB = speed_max * accele_l / 100;
}
/* Right motor */
if( accele_r >= 0 ) {
PBDR &= 0xf7;
ITU4_BRA = speed_max * accele_r / 100;
} else {
PBDR |= 0x08;
accele_r = -accele_r;
ITU4_BRA = speed_max * accele_r / 100;
}
}
/************************************************************************/
/* Servo handle operation */
/* Argument: Servo operation angle - -90 to 90 */
/* -90: turned 90亱 left, 0: straight ahead, 90: turned 90亱 right */
/************************************************************************/
void handle( int angle )
{
ITU4_BRB = SERVO_CENTER - angle * HANDLE_STEP;
}
/************************************************************************/
/* Bit reversal */
/* Argument: Value to be reversed */
/* Return value: Reversed value */
/************************************************************************/
char unsigned bit_change( char unsigned in )
{
unsigned char ret;
int i;
for( i = 0; i < 8; i++ ) {
ret >>= 1; /* Shifts the return value right*/
ret |= in & 0x80; /* ret bit7 = in bit7 */
in <<= 1; /* Shifts the argument left */
}
return ret;
}
/************************************************************************/
/* end of file */
/************************************************************************/?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -