#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=
评论0