Resa.rar_resa_舵机控制

#include <machine.h> #include "h8_3048.h" /*=====================================*/ /* Symbol definition */ /*======================================*/ /* Set Constants */ #define TIMER_CYCLE 2457 /* Timer 周期 0.1ms */ /* When it is to be used by f/8 */ /* f / 8 = 325.5[ns] */ /* Therefore, TIMER_CYCLE */ /* = 0.1[ms] / 325.5[ns] */ /* = 3072 *8/10 */ /* =2457 */ #define PWM_CYCLE 61439 /* PWM 周期 10ms */ /* Therefore, PWM_CYCLE */ /* = 2[ms] / 325.5[ns] */ /* = 49152 */ #define SERVO_CENTER 9756 /* Center value of Servo 4905 */ #define HANDLE_STEP 53 /* 0.1 degree part value 26 */ //#define KP 90 /*PID算法系数比例项KP0.1 */ //#define KI 22 /*PID算法系数积分项KI0.1 */ //#define KD 500 /*PID算法系数微分项KD 0.1 */ /*======================================*/ /* Prototype declaration */ /*======================================*/ void init(void ); void timer( unsigned long timer_set ); unsigned char sensor_inp( unsigned char mask ); unsigned char dipsw_get( void ); unsigned char pushsw_get( void ); void led_out( unsigned char led ); void speed( int accele_l, int accele_r ); void handle( int angle ); char unsigned bit_change( char unsigned in ); void handle_stop( int angle ); /*======================================*/ /* Global Variable Declaration */ /*======================================*/ unsigned long cnt0; /* used in 'timer' function */ //unsigned long cnt1; /* used in main */ unsigned int cnt2; /*辅助滤波计时变量*/ int sensor_e; /*滤波变量 */ int sensor_ee[4]; /*滤波变量存放数组 */ int sensor_p; /*滤波变量存放数组偏移指针变量*/ int sensor_sum; /*滤波变量存放数组求和 */ int sensor_avg; /*滤波变量平均值 */ int sensor_E; /*滤波求和变量,记录前一个状态*/ //int sensor_ci; /*PI算法中间量sensor_ci=(KI*sensor_e)/8+sensor_ci */ int sensor_cc; /*PI算法中积分量*/ int sensor_c; /*PI算法中间量sensor_c=(KP*sensor_e)/8+sensor_ci */ int sensor_D; /*微分保持器,微分瞬时值*/ int sensor_DD; /*微分保持器,微分保持值*/ int sensor_D_cnt; /*微分保持器,延时变量*/ int handle_angle; /*角度变量 一个单位0.1度*/ int handle_ang; /*角度变量中间量*/ int lock_direct; /*光电管在左临界状态时锁住变量-5~5*/ unsigned int lock_direct_control; unsigned int KI_left; /*积分快速补偿标志变量 1锁住0未锁住*/ unsigned int KI_right; /*积分快速补偿标志变量 1锁住0未锁住*/ int speed_down_cnt; /*微分量控制减速延迟计数量*/ long speed_down_cnt1; /*微分量控制,限速减速延迟计数量*/ unsigned int status; /*系统状态控制量 1：寻迹 2：准备进直角*/ /*3:检测到直角 左转弯 4:检测到直角 右转弯*/ unsigned int status_change; /*系统状态过度的计数变量*/ unsigned int status_cnt2; /*系统状态2的计数变量*/ unsigned int status_cnt3; /*系统状态3的计数变量*/ unsigned int status_cnt4; /*系统状态4的计数变量*/ unsigned int while_control; /*直角while循环控制量*/ int sensor_status; int sensor_now; int line_now; int line_befor; int line_status; int line_status_befor; unsigned long IRQ_right0; /*右轮外部中断函数计数0*/ unsigned long IRQ_right1; /*右轮外部中断函数计数1*/ unsigned int IRQ_right_cnt0; unsigned int IRQ_right_cnt00; /*右轮速度-时间计数变量 放1ms中断里*/ unsigned int real_speed_right; /*右轮当前速度,两次外部中断间隔42.4mm/(间隔时间) 量纲mm/s */ unsigned int cycle_speed_right; unsigned long IRQ_left0; /*左轮外部中断函数计数0*/ unsigned long IRQ_left1; /*左轮外部中断函数计数1*/ unsigned int IRQ_left_cnt1; unsigned int IRQ_left_cnt11; /*左轮速度-时间计数变量 放1ms中断里*/ unsigned int real_speed_left; /*左轮当前速度,两次外部中断间隔4.24cm/(间隔时间) 量纲cm/s*/ unsigned int cycle_speed_left; unsigned int sensor_D_cnt1; unsigned int sw_speed_max; unsigned int sw_speed_g; /************************************************************************/ /* Main program */ /************************************************************************/ void main( void ) { int i; int wait_key; /*等待按键变量 */ int angle; int speed_down_num; int one_speed; /*外轮速度-1000~1000 */ int other_speed; /*内轮速度-1000~1000 */ /* Microcomputer function initialization */ init(); /* Initialization */ set_ccr( 0x00 ); /* Whole interrupt enable */ /* State initialization of micom car */ handle( 0 ); speed( 0, 0 ); handle_stop( -100 ); cnt0 = 0;cnt2 = 0;//定时用计数器清零//cnt1 = 0; //所有变量初始化 handle_angle=0; angle=0;handle_ang=0; one_speed=0;other_speed=0; sensor_e=0; sensor_ee[0]=0;sensor_ee[1]=0;sensor_ee[2]=0;sensor_ee[3]=0; sensor_p=0; sensor_sum=0; sensor_avg=0; sensor_E=0; // sensor_ci=0; sensor_c=0; sensor_cc=0; KI_left=0; KI_right=0; lock_direct=0; lock_direct_control=0; sensor_D=0; sensor_DD=0; sensor_D_cnt=0; speed_down_cnt=3; speed_down_cnt1=0; speed_down_num=10; status=1; status_cnt2=0;status_cnt3=0;status_cnt4=0; status_change=1; while_control=1; sensor_status=0x02; sensor_now=0; line_now=1; line_befor=1; line_status=3; line_status_befor=3; IRQ_right0=0; IRQ_right1=0; IRQ_right_cnt0=0; IRQ_right_cnt00=0; real_speed_right=0; cycle_speed_right=0; IRQ_left0=0; IRQ_left1=0; IRQ_left_cnt1=0; IRQ_left_cnt11=0; real_speed_left=0; cycle_speed_left=0; sensor_D_cnt1=10000; sw_speed_max=350; sw_speed_g=20; sw_speed_max=(dipsw_get()&0x03)*50+200; sw_speed_g=((dipsw_get()&0x0c) )>>2; wait_key=1; led_out(3); while(wait_key){//等待按任意键 if( pushsw_get() ) wait_key=0; } timer(5000); led_out(0); sensor_D_cnt1=10000; speed_down_cnt=3; while(1){ //主循环 if((status_change==1) && (status!=1)){//如果是刚进入直角弯提示线 IRQ_left1=0; //等待 IRQ_right1=0; handle_ang=handle_angle; if(handle_angle>400)handle_ang=400; if(handle_angle<-400)handle_ang=-400; handle( handle_ang); while((IRQ_left1+IRQ_right1)<6){ i=(real_speed_left+real_speed_right)/2; if(i>250){speed(-900,-900); handle_stop( -300 );} else if(i>200){speed(-700,-700);handle_stop( -300 );} else if(i>150){speed(-200,-200);handle_stop( -100 );} else if(i>100){speed(0,0);handle_stop( -100 );} else {speed(500,500);handle_stop( -100 );} } handle_ang=handle_angle; if(handle_angle>400)handle_ang=400; if(handle_angle<-400)handle_ang=-400; handle( handle_ang); status=2; } switch(status){ //根据不同的工作状态执行不同操作 case 1: //直道 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=110;break; case 1:one_speed=100;other_speed=110;break; case 2:one_speed=100;other_speed=