resa.c

Resa智能车比赛时的源代码。传感器为车前排的光电管。一个舵机控制转向

				if(status_change==1||status_change==4){status=1;break;}		
						
			
				ITU0_IER = 0x00;//关中断
				while_control=1;
				i=(cycle_speed_left+cycle_speed_right)/2;
				if(i>=200){handle(-400);speed(-500,900);led_out(3);}
					else  if(i>=150){handle(-350); speed(-400,800);}
						else  if(i>=100){handle(-320); speed(50,800);}
							else {handle(-270); speed(100,800);led_out(0);}	
				handle_stop(-100);
				while(while_control){
					sensor_now=sensor_inp( 0xff );

					if(sensor_now&0xc0)sensor_status=(sensor_status&0x03)|0x04;
						else sensor_status=sensor_status&0x03;
					if(sensor_now&0x3c){sensor_status=(sensor_status&0x05)|0x02;line_now=1;}
						else {sensor_status=sensor_status&0x05;line_now=0;}
					if(sensor_now&0x03)sensor_status=(sensor_status&0x06)|0x01;
						else sensor_status=sensor_status&0x06;
								
					switch(line_befor-line_now){											
						case 1:if(sensor_status==0x04||sensor_status==0x00)line_status++;
								else if(sensor_status==0x01)line_status--;
							break;
						case -1:if(sensor_status==0x03)line_status++;
								else if(sensor_status==0x06)line_status--;
							break;	
											
						case 0:
						default:break;
						}
					if(line_status!=3)line_status_befor=line_status;
					if(line_status<=2){handle(50);speed(800,800);}
					if(line_status>=5){handle(-400);speed(-400,900);}
					if(line_status==3&&(sensor_now&0xe0)&&line_status_befor!=3)while_control=0;
					
								
					line_befor=line_now;

				}

				
				lock_direct=0;
				line_status_befor=3;
				ITU0_IER = 0x01;//开中断


																					
				while_control=1;				
				while(while_control){
				handle_ang=handle_angle;
				if(handle_angle>400)handle_ang=400;
				if(handle_angle<-400)handle_ang=-400;				
				handle(	handle_ang);	

				if(sensor_avg>0)angle=handle_angle/10;
					else angle=-handle_angle/10;		  											

				switch(	angle ){//根据转弯角度决定后轮差速
				case 0:one_speed=100;other_speed=100;break;
				case 1:one_speed=100;other_speed=99;break;
				case 2:one_speed=100;other_speed=97;break;
				case 3:one_speed=100;other_speed=96;break;
				case 4:one_speed=100;other_speed=95;break;
				case 5:one_speed=100;other_speed=93;break;
				case 6:one_speed=100;other_speed=92;break;
				case 7:one_speed=100;other_speed=91;break;
				case 8:one_speed=100;other_speed=89;break;
				case 9:one_speed=100;other_speed=88;break;
				case 10:one_speed=80;other_speed=87;break;
				case 11:one_speed=80;other_speed=86;break;
				case 12:one_speed=80;other_speed=84;break;
				case 13:one_speed=80;other_speed=83;break;
				case 14:one_speed=80;other_speed=82;break;
				case 15:one_speed=60;other_speed=81;break;
				case 16:one_speed=60;other_speed=79;break;  
				case 18:one_speed=60;other_speed=77;break;
				case 19:one_speed=60;other_speed=76;break;
				case 20:one_speed=60;other_speed=75;break;
				case 21:one_speed=50;other_speed=73;break;
				case 22:one_speed=50;other_speed=72;break;
				case 23:one_speed=50;other_speed=71;break;
				case 24:one_speed=50;other_speed=70;break;
				case 25:one_speed=50;other_speed=69;break;
				case 26:one_speed=50;other_speed=67;break;
				case 27:one_speed=50;other_speed=66;break;
				case 28:one_speed=50;other_speed=65;break;
				case 29:one_speed=50;other_speed=64;break;
				case 30:one_speed=50;other_speed=62;break;

				case 31:one_speed=40;other_speed=61;break;
				case 32:one_speed=40;other_speed=60;break;
				case 33:one_speed=40;other_speed=59;break;
				case 34:one_speed=40;other_speed=58;break;
				case 35:one_speed=30;other_speed=56;break;
				case 36:one_speed=30;other_speed=55;break;
				case 37:one_speed=30;other_speed=54;break;
				case 38:one_speed=30;other_speed=52;break;
				case 39:one_speed=20;other_speed=51;break;
				case 40:one_speed=20;other_speed=50;break;
				case 41:one_speed=20;other_speed=48;break;
				case 42:one_speed=20;other_speed=47;break;
				case 43:one_speed=20;other_speed=46;break;
				case 44:one_speed=20;other_speed=44;break;
				default:break;
				} 

				speed_down_num=9;
				if(handle_angle<0) 
					speed((other_speed*(one_speed/10)*speed_down_num)/10,one_speed*speed_down_num);												
					else speed(one_speed*speed_down_num,(other_speed*(one_speed/10)*speed_down_num)/10);
				
				if(angle<15)while_control=0;

				}
				while_control=1;


				status=1;
				status_change=3;
	    		break;

			case 4:			//右直角弯
				if(status_change==1||status_change==3){status=1;break;}
			
				ITU0_IER = 0x00;//关中断
				while_control=1;
				i=(cycle_speed_left+cycle_speed_right)/2;
				if(i>=200){handle(400);speed(900,-500);led_out(3);}
					else  if(i>=150){handle(350); speed(800,-400);}
						else  if(i>=100){handle(320); speed(800,50);}
							else {handle(270); speed(800,100);led_out(0);}	
				
				handle_stop(-100);
				while(while_control){
					sensor_now=sensor_inp( 0xff );
					if(sensor_now&0xc0)sensor_status=(sensor_status&0x03)|0x04;
						else sensor_status=sensor_status&0x03;
					if(sensor_now&0x3c){sensor_status=(sensor_status&0x05)|0x02;line_now=1;}
						else {sensor_status=sensor_status&0x05;line_now=0;}
					if(sensor_now&0x03)sensor_status=(sensor_status&0x06)|0x01;
						else sensor_status=sensor_status&0x06;
								
					switch(line_befor-line_now){											
						case 1:if(sensor_status==0x01||sensor_status==0x00)line_status++;
								else if(sensor_status==0x04)line_status--;
							break;
						case -1:if(sensor_status==0x06)line_status++;
								else if(sensor_status==0x03)line_status--;
							break;	
											
						case 0:
						default:break;
						}
					if(line_status!=3)line_status_befor=line_status;
					if(line_status<=2){handle(-50);speed(800,800);}
					if(line_status>=5){handle(400);speed(900,-400);	}
					if(line_status==3&&(sensor_now&0xe0)&&line_status_befor!=3)while_control=0;
					
								
					line_befor=line_now;

				}

				
				lock_direct=0;
				line_status_befor=3;
				ITU0_IER = 0x01;//开中断


																					
				while_control=1;				
				while(while_control){
				handle_ang=handle_angle;
				if(handle_angle>400)handle_ang=400;
				if(handle_angle<-400)handle_ang=-400;				
				handle(	handle_ang);	

				if(sensor_avg>0)angle=handle_angle/10;
					else angle=-handle_angle/10;		  											

				switch(	angle ){//根据转弯角度决定后轮差速
				case 0:one_speed=100;other_speed=100;break;
				case 1:one_speed=100;other_speed=99;break;
				case 2:one_speed=100;other_speed=97;break;
				case 3:one_speed=100;other_speed=96;break;
				case 4:one_speed=100;other_speed=95;break;
				case 5:one_speed=100;other_speed=93;break;
				case 6:one_speed=100;other_speed=92;break;
				case 7:one_speed=100;other_speed=91;break;
				case 8:one_speed=100;other_speed=89;break;
				case 9:one_speed=100;other_speed=88;break;
				case 10:one_speed=80;other_speed=87;break;
				case 11:one_speed=80;other_speed=86;break;
				case 12:one_speed=80;other_speed=84;break;
				case 13:one_speed=80;other_speed=83;break;
				case 14:one_speed=80;other_speed=82;break;
				case 15:one_speed=60;other_speed=81;break;
				case 16:one_speed=60;other_speed=79;break;
				case 17:one_speed=60;other_speed=78;break;
				case 18:one_speed=60;other_speed=77;break;
				case 19:one_speed=60;other_speed=76;break;
				case 20:one_speed=60;other_speed=75;break;
				case 21:one_speed=50;other_speed=73;break;
				case 22:one_speed=50;other_speed=72;break;
				case 23:one_speed=50;other_speed=71;break;
				case 24:one_speed=50;other_speed=70;break;
				case 25:one_speed=50;other_speed=69;break;
				case 26:one_speed=50;other_speed=67;break;
				case 27:one_speed=50;other_speed=66;break;
				case 28:one_speed=50;other_speed=65;break;
				case 29:one_speed=50;other_speed=64;break;
				case 30:one_speed=50;other_speed=62;break;

				case 31:one_speed=40;other_speed=61;break;
				case 32:one_speed=40;other_speed=60;break;
				case 33:one_speed=40;other_speed=59;break;
				case 34:one_speed=40;other_speed=58;break;
				case 35:one_speed=30;other_speed=56;break;
				case 36:one_speed=30;other_speed=55;break;
				case 37:one_speed=30;other_speed=54;break;
				case 38:one_speed=30;other_speed=52;break;
				case 39:one_speed=20;other_speed=51;break;
				case 40:one_speed=20;other_speed=50;break;
				case 41:one_speed=20;other_speed=48;break;
				case 42:one_speed=20;other_speed=47;break;
				case 43:one_speed=20;other_speed=46;break;
				case 44:one_speed=20;other_speed=44;break;
				default:break;
				} 

				speed_down_num=9;
				if(handle_angle<0) 
					speed((other_speed*(one_speed/10)*speed_down_num)/10,one_speed*speed_down_num);												
					else speed(one_speed*speed_down_num,(other_speed*(one_speed/10)*speed_down_num)/10);
				
				if(angle<15)while_control=0;

				}
				while_control=1;


				status=1;
				status_change=4;							
	    		break;
			default:
				break;
		}
	}
}

/************************************************************************/
/* H8/3048F-ONE Built in Peripheral Function Initialization             */
/************************************************************************/
void init( void )
{	/*系统时钟设置*/
	DIVCR=0xfc;
    /* I/O port Setting */
    P1DDR = 0xff;
    P2DDR = 0xff;
    P3DDR = 0xff;
    P4DDR = 0xff;
    P5DDR = 0xff;
    P6DDR = 0xf0;                       /* DIP SW on CPU board          */
    P8DDR = 0xfc;						/*     	两个外部中断			*/
    P9DDR = 0xf7;                       /* Communication Port           */
	PADR  = 0xbf;
    PADDR = 0xff;
    PBDR  = 0xc0;
    PBDDR = 0xfe;                       /* Motor Drive Board Vol.3      */
    /* As P7 of the sensor board is an exclusive input, there are no input output settings. */

    /* ITU0 Interrupt at every 1ms */
    ITU0_TCR = 0x20;
    ITU0_GRA = TIMER_CYCLE;
    ITU0_IER = 0x01;


	/*ITU2*/
	ITU2_TCR = 0x22;
	ITU2_GRA = PWM_CYCLE; 
	ITU2_GRB = 0;  
	ITU_MDR  =0x24;
	

    /* ITU3, 4 reset-synchronized PWM mode for right-left motor and servo */
    ITU3_TCR = 0x22;
    ITU_FCR  = 0x3e;
    ITU3_GRA = PWM_CYCLE;                   /* Setting of cycle         */
    ITU3_GRB = ITU3_BRB = 0;                /* PWM Setting of left  motor*/
    ITU4_GRA = ITU4_BRA = 0;                /* PWM Setting of right motor*/


    ITU4_GRB = ITU4_BRB = SERVO_CENTER;     /* PWM Setting of servo     */
    ITU_TOER = 0x38;

    /* Count start of ITU */
    ITU_STR = 0x0d;

	/*IRQ0，IRQ1两个外部中断初始化*/
	IER=0x03;
	ISCR=0x03;
	

	/*中断优先级设置*/

	IPRA=0xc0;


}

/************************************************************************/
/* ITU0 Interrupt process                                               */
/************************************************************************/
#pragma interrupt( interrupt_timer0 )
void interrupt_timer0( void )
{	int Given;
    ITU0_TSR &= 0xfe;                   /* Flag clear                   */
    cnt0++; cnt2++;
	speed_down_cnt--;
	speed_down_cnt1--;
	sensor_D_cnt1--;
	
	IRQ_right_cnt0++;
	IRQ_right_cnt00++;
	IRQ_left_cnt1++;
	IRQ_left_cnt11++;
	


	/*	switch(handle_angle/100){
		case 0:	Given=0;break;

		case 1:	Given=-5;break;
		case 2: Given=-17;break;
		case 3: Given=-22;break;
		case 4: Given=-22;break;

		case -1: Given=5;break;
		case -2: Given=17;break;
		case -3: Given=22;break;
		case -4: Given=22;break;
		default: Given=0;break;

	}
*/	
	Given=0;
	switch(sensor_inp( 0xff )){//根据光电检测结果确定偏差量

		case 0x18:switch(lock_direct){
					case -1:case 0:case 1:sensor_e=Given;lock_direct=0;break;
					case -3:case -4:case -5:sensor_e=Given-70;lock_direct=-4;break;
					case 3:	case 4:	case 5:sensor_e=Given+70;lock_direct=4;break;
					default:break;}break;		/*	00011000	*/								
		case 0x81:switch(lock_direct){
					case-1:case -2:	case -3:sensor_e=Given-65;lock_direct=-2;break;
					case 1:	case 2:	case 3:sensor_e=Given+65;lock_direct=2;break;
					default:break;}break;		/*	10000001	*/	
		//偏左
		case 0x1c:
		case 0x08: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;		/*	00001000	*/
		case 0x0c: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;		/*	00001100	*/
		case 0x0e:
		case 0x04: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;		/*	00000100	*/