📄 resa.c
字号:
#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=110;break;
case 3:one_speed=100;other_speed=110;break;
case 4:one_speed=100;other_speed=110;break;
case 5:one_speed=100;other_speed=110;break;
case 6:one_speed=100;other_speed=110;break;
case 7:one_speed=100;other_speed=110;break;
case 8:one_speed=100;other_speed=110;break;
case 9:one_speed=100;other_speed=110;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=50;break;
case 32:one_speed=40;other_speed=50;break;
case 33:one_speed=40;other_speed=50;break;
case 34:one_speed=40;other_speed=40;break;
case 35:one_speed=30;other_speed=40;break;
case 36:one_speed=30;other_speed=40;break;
case 37:one_speed=30;other_speed=30;break;
case 38:one_speed=30;other_speed=30;break;
case 39:one_speed=20;other_speed=30;break;
case 40:one_speed=20;other_speed=30;break;
case 41:one_speed=20;other_speed=30;break;
case 42:one_speed=20;other_speed=30;break;
case 43:one_speed=20;other_speed=30;break;
case 44:one_speed=20;other_speed=30;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;
}
/*微分量控制减速*/
if(sensor_D_cnt1>2)sensor_D=0;
else sensor_D_cnt1=1;
if(sensor_D>6||sensor_D<-6){
speed_down_cnt=1000;
}
if(speed_down_cnt>200)speed_down_num=0;
else if(speed_down_cnt>100)speed_down_num=2;
else if(speed_down_cnt>3)speed_down_num=7;//0.1比例
else { speed_down_num=10;
speed_down_cnt=2;
}
/*差速调整*/
//if(sensor_avg<39&&sensor_avg>-39);
//else if( (sensor_avg>19&&sensor_avg<39)||(sensor_avg<-19&&sensor_avg>-39))other_speed-=25;
// else if( sensor_avg>39&&sensor_avg<-39)other_speed-=25;
if(speed_down_cnt1>30000&&((real_speed_right+real_speed_left)/2)>150){
led_out(3);
speed_down_num=3*speed_down_num/10;}
else if(speed_down_cnt1>15000&&((real_speed_right+real_speed_left)/2)>150){
led_out(3);
speed_down_num=5*speed_down_num/10;}
else if(speed_down_cnt1>2&&((real_speed_right+real_speed_left)/2)>200){
led_out(3);
speed_down_num=7*speed_down_num/10;}
else {
led_out(0);
speed_down_num=10*speed_down_num/10;
speed_down_cnt1=0;}
if( ((cycle_speed_right+cycle_speed_left)>>1)>sw_speed_max)speed_down_num=7*speed_down_num/10;
else speed_down_num=speed_down_num*10/10;
if(handle_angle<0)
speed(((other_speed-10)*(one_speed/10)*speed_down_num)/10,one_speed*speed_down_num);
else speed(one_speed*speed_down_num,((other_speed-10)*(one_speed/10)*speed_down_num)/10);
if(sensor_e>=53){speed(-200,70*speed_down_num);speed_down_cnt1=40000;}
if(sensor_e<=-53){speed(70*speed_down_num,-200);speed_down_cnt1=40000;}
// if(lock_direct>=2){speed(-200,70*speed_down_num);speed_down_cnt1=60000;}
//if(lock_direct<=-2){speed(70*speed_down_num,-200);speed_down_cnt1=60000;}
status_change=1;
break;
case 2: //直角弯准备,减速寻迹
if(status_change==3||status_change==4){status=1;break;}
handle_ang=handle_angle;
if(handle_angle>400)handle_ang=400;
if(handle_angle<-400)handle_ang=-400;
handle( handle_ang);
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 );}
status_change=2;
break;
case 3: //左直角弯
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -