基于PID闭环控制的智能车反馈系统研究与
设计
领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计
摘要:本智能车系统设计以瑞萨公司开发的H8-3048为主控制器，通
过红外线传感器检测小车的位置和运动方向，光电编码器检测模型车
的速度。PID控制算法调节驱动电机的转速和舵机的方向，完成对模
型车运动速度和运动方向的闭环控制。实验结果
表
关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf
明，系统设计
方案
气瓶 现场处置方案 .pdf气瓶 现场处置方案 .doc见习基地管理方案.doc关于群访事件的化解方案建筑工地扬尘治理专项方案下载
可行。
关键词:H8-3048;红外线传感器; PID;
ABSTRACT: The design of the intelligent vehicle system with the key controler H8-3048 which was developped by Renesas Corporation,to
regonize the vehicle’s location and movement direction by an array of infrared sensors,and to direct the vehicle’s speed
by the optical encoders.The PID algorithm are utilezed to regulate the speed of the motor-drive and the direction of the servomotor,to Complete the closed-loop control of the model vehicle’s speed and direction.The experimental results show that the system design program is feasible.
KEY WORDS H8-3048;Infrared sensors;PID;
I
目录
1.前言 ................................................... 1
1.1设计制作思路 ............................................................................................. 1
1.2设计方案概述 ............................................................................................. 3 2.硬件部分 ............................................... 5
2.1车模组装 ..................................................................................................... 5
2.2传感器的设计与安装 ................................................................................. 5
2.2.1光电传感器的选择及工作原理...................................................... 5
2.2.2光电传感器的布置方案 ................................................................. 6
2.2.3光电传感器信号探测及处理电路 .................................................. 7
2.2.4速度传感器的安装 ......................................................................... 8
2.3系统电路板的设计、安装与连接 .............................................................. 8
2.4驱动电机和舵机 ......................................................................................... 8
2.4.1工作原理 .......................................................................................... 8
2.4.2电路设计 .......................................................................................... 9
2.5主要车模参数 .......................................................................................... 11 3.软件部分 .............................................. 12
3.1控制算法 ................................................................................................... 12
3.1.1算法介绍 ........................................................................................ 12
3.1.2程序实现 ...................................................................................... 15 4.结论 .................................................. 15
4.1主要性能 .................................................................................................. 16
4.2改进方向 ................................................................................................... 16 致谢…………………………………………………………………………………16 参考文献 ................................................ 17 附录:程序源代码 ........................................ II
1.前 言
随着人们生活水平的提高，人们对智能控制的需求与日俱增。智能控制五花八门，从工业控制到日常生活的边边角角都有智能控制的身影。
目前，随着人们拥有轿车数量的提升，人们对轿车安全性的要求将会日益提高，轿车的智能控制系统将作为轿车安全性的重要组成部分。而对于智能车控制系统的开发还处于研究阶段，因此，基于PID闭环控制的智能车反馈系统研究与设计，具有十分重要的现实意义和应用价值。
目前大批的仪表与过程工程师们都熟悉PID控制，而且已经建立了一套很好的安装、整定和使用PID控制器的方法。尽管这样，发展PID控制仍然有很大的潜力，这是因为许多控制器在实际中都处在手动状态，而那些处在自动状态的控制器由于微分作用不好调整往往把微分环节去掉。PID控制器在实际中性能欠佳的主要原因一是阀门和传感器的设备问
题
快递公司问题件快递公司问题件货款处理关于圆的周长面积重点题型关于解方程组的题及答案关于南海问题
，另外就是较差的整定实践。提高控制回路的性能，不仅需要PID控制的知识，而且需要过程方面的知识。
PID参数的整定一般需要经验丰富的工程技术人员来完成，既耗时又耗力，加之实际系统千差万别，又有滞后、非线性等因素，使PID参数的整定有一定的难度，致使许多PID控制器没能整定的很好，这样的系统自然无法工作在令人满意的状态，为此人们提出了自整定PID控制器。将过程动态性能的确定和PID控制器参数的计算方法结合起来就可实现PID控制器的自整定。自整定的含义是控制器的参数可根据用户的需要自动整定,用户可以通过按动一个按钮或给控制器发送一个命令来启动自整定过程。
本课题主要解决PID控制算法在只能小车上的参数整定问题，使智能小车基于PID闭环控制的智能反馈系统，能在直道上保持高速移动的同时并在弯道上能加速过弯。
2. 硬件部分
2.1车模组装
严格按照设计说明对车模进行组装。
严格按照检测说明对车模进行检测，调整车模的硬件参数。
参数调整中最主要的一定要调整舵机的初始化角度，使舵机的初始化角度与车模的中轴线平行。
2.2传感器的设计与安装
2.2.1光电传感器的选择及工作原理
智能车系统以一个标准的汽车模型作为载体。为了提高智能车行驶的速度和稳定性, 单片机需要将路径的判断、舵机与驱动电机的控制紧密地结合在一起。而路径识别模块是智能车系统的关键模块之一, 路径识别方案多采用红外反射式光电传感器或者CCD/CMOS图像传感器。CCD/CMOS图像传感器寻迹方案的优点是可以更远更早地感知赛道的变化, 但是信号处理比较复杂。光电传感器体积小、价格低廉、安装灵活方便且不易受可见光的干扰, 故选用红外反射式光电传感器作为路径检测元件。
工作原理如图2.2所示:
图2.2 传感器工作原理图
3
红外反射式光电传感器由一个发射管和一个接收管组成, 相应于不同的路面条件(主要是黑白度), 接收管接收到地面漫反射红外线后其两端电压将有所不同。其电压大小与路面情况有关:当路面的颜色为白色时，反射光较强, 电压较高;当路面的颜色为黑色时，反射光较弱, 电压较低。因此,可以根据传感器电压的大小获得智能车的路面信息。
光电传感器使用过程中要注意几个问题。首先, 要提高器件的一致性:一是发射/接收器件两者的波长特性一致;二是发射/接收传感器组对时, 各方面性能应尽量比较接近;三是安装状态一致, 如各对传感器的间距、高低、偏角等, 因为这些因素都会影响光的反射和接收。其次, 光电传感器应尽量靠近地面, 以便减少外界光源对传感器的干扰。垂直高度为5-10mm为宜, 离地面过远, 光反射信号差, 信号不强;离地面过近, 会导致反射角度太大, 加剧光漫射干扰的影响, 故应适当, 以保证传感器具有最佳的反应。
2.2.2光电传感器的布置方案
传感器的布置方案有单排、双排、W字形排列等。我们采用了单排一字形的排列方式。
具体的布置方式如图2.3，也就是在车模的头部安装一块电路板，其底部安装8个传感器。我们用编码的形式把传感器探测到的路面信息表示出来，然后把这些路面信息输入到控制系统，通过编写的控制算法就可以控制舵机和驱动电机，实现智能车的加速、减速、左转、右转了。通过不断的实验，来进一步地调整各种编码下，智能车要转的角度的参数。我们采用PID控制算法，它可以解决智能车行驶速度与转向角度大小的相关性问题，行驶轨迹的跟踪及智能车偏离预定轨迹时的调整问题等。为了最大限度地增大传感器的预描距离，在满足组委会规定的
尺寸
手机海报尺寸公章尺寸朋友圈海报尺寸停车场尺寸印章尺寸
内，我们让传感器的安装位置尽量向前靠。
图2.3
6
带传感器的电路板的布置如图2.4所示:两个相邻的传感器的中心距离位18mm;
图2.4
2.2.3光电传感器信号探测及处理电路
传感器信号探测及处理电路设计如图2.5所示:
图2.5 传感器探测与处理电路图
7
2.2.4速度传感器的安装
速度传感器安装于车模后部，使取速齿轮与赛道接触，当小车跑动时通过与赛道的接触以带动取速齿轮运动，从而获得车模的速度，速度传感器的安装与固定要考虑到信号的准确性和稳定性。
2.3系统电路板的设计、安装与连接
我们采用主办方提供的H8-3048单片机如图2.7所示:
图2.7
2.4驱动电机和舵机
2.4.1工作原理
智能车系统以H8-3048为核心, 由单片机模块、电源管理模块、传感器模块、直流电机驭动模块、舵机控制模块等组成, 如图2.10所示。单片机通过光电传感器阵列采集路径信息, 然后采用一定的控制算法控制舵机的转角和直流驱动电机的转速, 从而实现智能车自动寻迹行驶。
8
图2.10小车工作原理原理图
图2.11 车模电机驱动控制板图 2.4.2电路设计
电机驱动板的电路图如图2.11所示。
图2.11 电机驱动板电路图
9
PWM( Pulse Width Modulation)，又称脉冲宽度调制方式，是指通过改变输入脉冲的有效工作时间，改变输入平均电压的调节电机转速方式。PWM信号的有效工作时间和周期的比值为PWM信号的占空比a为:
a= to/T out
式中
a—占空比值
—PWM一个周期内高低电平中有效电平时间 toout
T—PWM周期
本设计的舵机采用PWM脉冲控制，通过调整脉冲的占空比实现舵机的左转和右转，如图2.12所示。
图2.12 舵机工作原理图
本设计用H桥芯片MC33886驱动直流电机。由于电机启动和快速加速时所需电流较大，采用两片MC33886并联工作,并且采用PWM控制电机的转速。电路图如图2.13和2.14所示。
图2.13 电机驱动电路图
10
图2.14 电机PWM调节电路图
本设计中的各个硬件模块的设计为车模的行驶稳定性和快速性提供了坚实的基础，各个功能模块的调试与功能实现都符合大赛相关规定。
2.5主要车模参数
a)车重:1.5kg 长:384mm 宽:185mm 高:47mm
b)传感器种类:反射式红外光电传感器 数目:8个
c)舵机个数:1个
d)电机个数:2个
11
3.软件部分
3.1控制算法
由于采集到的路面信息为离散值, 如果直接应用到转向及车速控制策略中, 势必造成转向及车速调节的阶跃式变化, 这将会使智能车的转向及车速控制僵硬, 对路径变化反应不灵敏,以及易产生超调及振荡现象。由于舵机输出转角相对于路径为阶跃式延迟响应, 对于追求高速性能的高车速短决策周期的控制策略来说, 很可能因为舵机响应不及而造成控制失效。因而决定采用基于离散路面信息的连续控制算法, 即舵机的PID控制算法和驱动电机的间接PID控制算法。
3.1.1算法介绍
PID控制算法包括直接计算法和增量算法，所谓的增量算法就是相对于标准的相邻两次运算之差，得到的结果是增量。也就是说在上一次的控制量的基础上需要增加(负值意味着减少)控制量，例如对于智能小车控制算法，就是智能小车相对于上一次转向角度还需要增加或减少的转向角度。在本方案的设计中采用了PID增量法。
算法: (一)舵机PID控制
T=T+?Tkk-1k
?T=K(e-e)+Ke+K[(e-e)-( e-e)] kPKk-1IDKk-1k-1K-2
式中:T 和T为当前和上次操作量;?T为当前操作微分; kk-1k
e、e、e为当前、上次、上上次偏差; Kk-1K-2
K、K、K是做好控制器的关键常数，分别称为比例常数、积分常数和微PID
分常数，不同的控制对象需要选择不同的数值，需要经过现场调试才能
获得较好的效果。
为了避免系统可能出现的积分饱和现象, 对PID控侧器的积分部分做了两个改进:积分分离法和遇限削弱积分法。
积分分离法的思路是当被控与给定值的偏差较大时, 去掉积分, 以避免积分饱和效应的产生;当被控与给定值比较接近时, 重新引人积分, 发挥积分的作用, 消除静态误差, 从而既保证了控制的精度又避免了振荡的产生。为此在积分项中乘以一个人为引人的系数, 公式可改写为
?T=K(e-e)+PKe+K[(e-e)-( e-e)] kPKk-1IDKk-1k-1K-2
式中:当|e|
e1){
e1=e0;
e2=p*e0/cnt1; sum+=e2;
}
if(e0pmax) return pmax; if(sum
#include "h8_3048.h"
/*======================================*/
/* Symbol definitions */
/*======================================*/
/* Constants */
#define TIMER_CYCLE 3071 /* Timer cycle: 1 ms */
/* When &/8 is used, */
/* &/8 = 325.5 ns */
/* TIMER_CYCLE = */
/* 1[ms] / 325.5[ns] */
/* = 3072 */
#define PWM_CYCLE 49151 /* PWM cycle: 16 ms */
/* PWM_CYCLE = */
/* 16[ms] / 325.5[ns] */
III
/* = 49152 */
#define SERVO_CENTER 4490/* Servo sensor value 4880*/
#define HANDLE_STEP 26 /* Value for 1 degree */
/* Mask value setting: x: Masked (disabled) O: Not masked (enabled) */ #define MASK2_2 0x66 /* */ #define MASK2_0 0x60 /* */ #define MASK0_2 0x06 /* */ #define MASK3_3 0xe7 /* */ #define MASK0_3 0x07 /* */ #define MASK3_0 0xe0 /* */ #define MASK4_0 0xf0 /* */ #define MASK0_4 0x0f /* */ #define MASK4_4 0xff
#define MASK0_0 0x18 /* */ #define MASK1_1 0x81
/*my define*/
#define PID_TYPE float
#define TIMER1_CYCLE 614
#define PI 3.14
#define D 45 /*后轮直径*/
/*======================================*/
/* Prototype declarations */
IV
/*======================================*/ void init( void );
void timer( unsigned long timer_set ); int check_crossline( void );
int check_rightline( void );
int check_leftline( void );
unsigned char sensor_inp( unsigned char mask ); unsigned char dipsw_get( void );
unsigned char pushsw_get( void ); unsigned char startbar_get( void ); void led_out( unsigned char led ); void speed( int accele_l, int accele_r ); void handle( float angle );
char unsigned bit_change( char unsigned in );
int getlspeed();
int getrspeed();
void pid_init(struct pid_unit *unit, // PID控制模块指针
PID_TYPE p, // 比例系数
PID_TYPE i, // 积分系数
PID_TYPE d); // 微分系数
PID_TYPE pid_control(struct pid_unit *unit, // PID控制模块
PID_TYPE input, // 当前输入数据
PID_TYPE time); // 距离上个数据的时间间隔
/*======================================*/
/* Global variable declarations */ V
/*======================================*/ unsigned long cnt0; /* For the timer functions */
unsigned long cnt1; /* Used in main() */ int pattern; /* Pattern number */
unsigned long cnt2; /* Used in main() */ unsigned char newsensor;
unsigned char oldsensor;
unsigned long cnt_lsec; /*左马达一个脉冲用时*/ unsigned long cnt_rsec; /*右马达一个脉冲用时*/ unsigned long this_cnt_lsec; /*左马达一个脉冲用时*/ unsigned long this_cnt_rsec; /*右马达一个脉冲用时*/ float lspeed; /*左马达速度*/
float rspeed; /*右马达速度*/
float ltemp; /*左马达临时速度*/
float rtemp; /*右马达临时速度*/
int pidlspeed;
int pidrspeed;
// PID控制模块(每个控制单元需要一个此结构)
struct pid_unit {
PID_TYPE pid_sens[2]; // 保存连续最新3个输入数据序列
PID_TYPE pid_cpid[3]; // 比例、积分、微分系数
PID_TYPE pid_dpid[3]; // PID变量
};
VI
//变速用
unsigned char sw_data;
int K;
/************************************************************************/
/* Main Program */
/************************************************************************/
void main( void )
{
struct pid_unit unit1;
/* Microcontroller function initialization */
init(); /* Initialization */
set_ccr( 0x00 ); /* Enable all interrupts */
/* Micom car state initialization */
handle( 0 );
speed( 0 ,0 );
sw_data = dipsw_get() + 5;
while( 1 )
{
switch( pattern )
VII
{
/*****************************************************************
Notes on patterns
0: Wait for switch input
1: Check whether or not the start bar is open
11: Normal trace
12: Check for the end of a wide turn to right
13: Check for the end of a wide turn to left
21: Processing for when the first crossline is detected
22: Read and jump over the second
23: Detect a trace or sharp turn after a crossline
31: Left sharp turn clear processing: Wait briefly until clear
32: Left sharp turn clear processing: Check that the end of the turn has been reached
41: Right sharp turn clear processing: Wait briefly until clear
42: Right sharp turn clear processing: Check that the end of the turn has been
reached
51: Processing when the first right half-line has been detected
52: Read and jump over the second
53: Trace following a right half line
54: Check that the right lane change has completed
61: Processing when the first left half-line has been detected
62: Read and jump over the second
63: Trace following a left half line
64: Check that the left lane change has completed
*****************************************************************/
case 0:
/* Wait for switch input */
if( pushsw_get() )
VIII
{
pattern = 1;
cnt1 = 0;
break;
}
if( cnt1 < 100 )
{ /* Processing for LED blinking */
led_out( 0x1 );
}
else
if( cnt1 < 200 )
{
led_out( 0x2 );
}
else
{
cnt1 = 0;
}
break;
case 1:
/* Check whether or not the start bar is open */
if( !startbar_get() )
{
/* Start!! */
led_out( 0x0 );
pattern = 2;
cnt1 = 0;
break;
IX
}
if( cnt1 < 50 )
{ /* Processing for LED blinking */
led_out( 0x1 );
}
else
if( cnt1 < 100 )
{
led_out( 0x2 );
}
else
{
cnt1 = 0;
}
break;
case 2:
if( check_crossline() || check_rightline() || check_leftline() ) {
pattern = 3;
timer(10);
break;
}
if(cnt2>50)
{
oldsensor=newsensor;
X
newsensor=sensor_inp(MASK3_3);
cnt2=0;
}
switch( sensor_inp(MASK3_3) )
{
case 0x00:
/* Center 仺 straight */
handle( 0 );
speed( 100 ,100 );
break;
case 0x04:
/* Move slightly to the left 仺 turn slightly to the right */
handle( 5 );
speed( 100 ,100 );
break;
case 0x06:
/* Move somewhat to the left 仺 turn somewhat to the right */
handle( 18 );
speed( 100 ,80);
break;
case 0x07:
/* Move a medium amount to the left 仺 turn a medium amount to the right */
handle( 25 );
speed( 90 ,60);
XI
break;
(最厉害)*/ case 0x03: /*偏左等级1
if(oldsensor==0x00||oldsensor==0x04)
{
led_out(0x3);
handle( 44 );
pid_init(&unit1,35,8,8);
pattern=14;
}
else
{
handle( 38 );
speed( 60,22 );
}
break;
case 0x20:
/* Move slightly to the right 仺 turn slightly to the left */
handle( -5);
speed( 100 ,100 );
break;
case 0x60:
/* Move somewhat to the right 仺 turn somewhat to the left */
handle( -18 );
speed( 80 ,100 );
break;
XII
case 0xe0:
/* Move a medium amount to the right 仺 turn a medium amount to the left */
handle( -25 );
speed( 60 ,90 );
break;
case 0xc0: /*偏右等级1(最厉害)*/
if(oldsensor==0x00||oldsensor==0x20)
{
led_out(0x3);
handle( -44 );
pid_init(&unit1,35,8,8);
pattern=15;
}
else
{
handle( -38 );
speed( 22 ,60 );
}
break;
default:
break;
}
break;
XIII
case 3:
if ( sensor_inp(MASK1_1)==0x81 ){ //判断直角弯
pattern=21;
handle(0);
cnt1=0;
break;
}
右变线区 if(sensor_inp(MASK1_1)==0x01){ //判断
pattern=6;
cnt1=0;
break;
}
if(sensor_inp(MASK1_1)==0x80){ //判断左变线区
pattern=5;
cnt1=0;
}
break;
/*左变线区转弯程序*/
case 5:
if(sensor_inp(MASK1_1)==0x01 && cnt1<700){
pattern=21;
cnt1=0;
break;
}
if(sensor_inp(MASK4_4)==0x00){
pattern=51;
XIV
handle( -18 );
speed( 30 ,50 );
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
/* Center straight */
handle( 0 );
speed( 90 ,90 );
break;
case 0x04:
handle( 5 );
speed( 80 ,80 );
break;
case 0x06:
case 0x07:
case 0x03:
/* Move left turn right */
handle( 8 );
speed( 70 ,70 );
break;
case 0x20:
case 0x60:
handle( -5 );
speed( 80 ,80 );
break;
case 0xe0:
case 0xc0:
XV
/* Move right turn left */
handle( -8 );
speed( 70 ,70 );
break;
}
break;
/*右变线区转弯程序*/
case 6:
if(sensor_inp(MASK1_1)==0x80 && cnt1<700){
pattern=21;
break;
}
if(sensor_inp(MASK4_4)==0x00){
pattern=61;
handle( 18 );
speed( 50 ,30 );
break;
}
if( sensor_inp(MASK4_4)==0xf0 || sensor_inp(MASK4_4)==0xf8 ){
handle( -48 );
if(cnt1<20) speed( 0 ,0 );
if(cnt1<80 && cnt1>20) speed( -30 ,30 );
if(cnt1>80) speed( -60 ,30 );
pattern = 31;
cnt1 = 0;
break;
}
XVI
if( sensor_inp(MASK4_4)==0x0f || sensor_inp(MASK4_4)==0x1f ) {
handle( 48 );
if(cnt1<20) speed( 0 ,0 );
if(cnt1<80 && cnt1>20) speed( 30 ,-30 );
if(cnt1>80) speed( 30 ,-60 );
pattern = 41;
cnt1 = 0;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
/* Center straight */
handle( 0 );
speed( 90 ,90 );
break;
case 0x04:
handle( 5 );
speed( 80 ,80 );
break;
case 0x06:
case 0x07:
case 0x03:
/* Move left turn right */
handle( 8 );
speed( 70 ,70 );
break;
case 0x20:
case 0x60:
XVII
handle( -5 );
speed( 80 ,80 );
break;
case 0xe0:
case 0xc0:
/* Move right turn left */
handle( -8 );
speed( 70 ,70 );
break;
}
break;
case 14:
/* Check for completion of a large turn to the right. */
if( check_crossline() )
{ /* Also check the crossline during large turns. */
pattern = 21;
break;
}
if( check_leftline() )
if(sensor_inp(MASK4_4)==0x00){
pattern=61;
handle( -15 );
speed( 30 ,50 );
XVIII
break;
}
if( check_rightline() )
if(sensor_inp(MASK4_4)==0x00){
pattern=61;
handle( 15 );
speed( 50 ,30 );
break;
}
/*检测右急弯出弯*/
getrspeed();
pidrspeed=pid_control(&unit1,1.5-rspeed,0.0002); speed( (int)pidrspeed ,(int)(pidrspeed - 30) ); if( sensor_inp(MASK3_3) == 0x06||sensor_inp(MASK3_3) == 0x07||sensor_inp(MASK3_3)
== 0x04||sensor_inp(MASK3_3) == 0x00)
{
pattern = 2;
led_out(0x0);
break;
}
break;
case 15:
/* Check for completion of a large turn to the left. */
if( check_crossline() )
XIX
{ /* Also check the crossline during large turns. */
pattern = 21;
break;
}
if( check_leftline() )
if(sensor_inp(MASK4_4)==0x00){
pattern=61;
handle( -15 );
speed( 30 ,50 );
break;
}
if( check_rightline() )
if(sensor_inp(MASK4_4)==0x00){
pattern=61;
handle( 15 );
speed( 50 ,30 );
break;
}
/*检测左急弯出弯*/
getrspeed();
pidlspeed=pid_control(&unit1,1.5-rspeed,0.0002);
speed( (int)pidrspeed - 30 ,(int)pidrspeed );
if( sensor_inp(MASK3_3) == 0x60||sensor_inp(MASK3_3) == 0xc0 ||sensor_inp(MASK3_3)
== 0x20||sensor_inp(MASK3_3) == 0x00)
{
XX
pattern = 2;
led_out(0x0);
break;
}
break;
case 21:
/* Processing when the first crossline is detected */
handle( 0 );
// pid_init(&unit1,10,1,1);
speed( 0 ,0 );
/*
do
{
getrspeed();
pidrspeed=pid_control(&unit1,2-rspeed,0.0002);
speed( (int)pidrspeed ,(int)(pidrspeed) );
}
while(cnt1>50);
*/
pattern = 22;
cnt1 = 0;
break;
case 22:
if( cnt1 < 20 ) speed(50,50);
if( cnt1 < 30 && cnt1>20 ) speed(50,50);
if( cnt1 < 40 && cnt1>30) speed(40,40); XXI
if( cnt1 < 50 && cnt1>40) speed(40,40);
if( cnt1 < 100 && cnt1>50) speed(40,40);
if( cnt1 < 120 && cnt1>100)
{
pattern = 23;
cnt1 = 0;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
handle( 0 );
break;
case 0x04:
handle( 5 );
break;
case 0x06:
case 0x07:
case 0x03:
handle( 8 );
break;
case 0x20:
case 0x60:
handle( -5 );
break;
case 0xe0:
case 0xc0:
handle( -8 );
break;
}
break;
XXII
case 23:
if( (sensor_inp(MASK4_4)==0xf0 || sensor_inp(MASK4_4)==0xf8)&&cnt1>100 ){
handle( -48 );
if(cnt1<20) speed( 5 ,10 );
if(cnt1<80 && cnt1>20) speed( 10 ,20 );
if(cnt1>80) speed( 10 ,30 );
pattern = 31;
cnt1 = 0;
break;
}
if( (sensor_inp(MASK4_4)==0x0f || sensor_inp(MASK4_4)==0x1f)&&cnt1>100 )
{
handle( 48 );
if(cnt1<20) speed( 10 ,0 );
if(cnt1<80 && cnt1>20) speed( 20 ,10 );
if(cnt1>80) speed( 30 ,10 );
pattern = 41;
cnt1 = 0;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
/* Center straight */
handle( 0 );
// speed( 50 ,50 );
break;
XXIII
case 0x04:
handle( 5 );
speed( 40 ,40 );
break;
case 0x06:
case 0x07:
case 0x03:
/* Move left turn right */
handle( 8 );
speed( 30 ,30 );
break;
case 0x20:
case 0x60:
handle( -5 );
speed( 40 ,40 );
break;
case 0xe0:
case 0xc0:
/* Move right turn left */
handle( -8 );
speed( 30 ,30 );
break;
}
break;
case 31:
if( cnt1 > 200 ) {
pattern = 32;
cnt1 = 0;
XXIV
}
break;
case 32:
if( sensor_inp(MASK3_3) == 0x20 ) {
pattern = 2;
cnt1 = 0;
}
break;
case 41:
if( cnt1 > 200 ) {
pattern = 42;
cnt1 = 0;
}
break;
case 42:
/* Right sharp turn clear processing - check for completion of the turn
*/
if( sensor_inp(MASK3_3) == 0x04 ) {
pattern = 2;
cnt1 = 0;
}
break;
case 51:
if( sensor_inp( MASK4_4 ) == 0xe0 || sensor_inp( MASK4_4 ) == 0xc0 ){
XXV
do{
handle(10);
speed(70,60);
}
while(sensor_inp( MASK3_3 ) != 0x00 && sensor_inp( MASK3_3 ) != 0x20
&& sensor_inp( MASK3_3 ) != 0x04 && sensor_inp( MASK3_3 ) != 0x06);
cnt1 = 0;
handle(0);
pattern=2;
K=0;
}
break;
case 61:
if( sensor_inp( MASK4_4 ) == 0xe0 || sensor_inp( MASK4_4 ) == 0xc0 ){
do{
handle(-10);
speed(70,60);
}
while( sensor_inp( MASK3_3 ) != 0x00 && sensor_inp( MASK3_3 ) !=
0x20 && sensor_inp( MASK3_3 ) != 0x04);
cnt1 = 0;
handle(0);
pattern=2;
K=0;
}
break;
XXVI
case 90:
sw_data = 19;
led_out( 0x0 );
switch( sensor_inp(MASK3_3) ) {
case 0x00:
do{
handle( 0 );
speed( 100 ,100 );
K++;
if (K>2000)
{
pattern=2;
led_out( 0x1 );
sw_data = dipsw_get() + 5;
break;
}
}
while(sensor_inp(MASK3_3)==0x00);
break;
case 0x03:
handle( 40 );
speed(60,10);
break;
case 0xc0:
handle( -40 );
speed(10,60);
XXVII
break;
case 0x04:
handle( 5 );
speed( 100 ,100 );
break;
case 0x20:
handle( -5);
speed( 100 ,100);
break;
case 0x06:
do{
handle( 20 );
speed( 100 ,70 );
}
while(sensor_inp(MASK3_3)==0x06);
break;
case 0x60:
do{
handle( -20);
speed( 70 ,100 );
}
while(sensor_inp(MASK3_3)==0x60);
break;
case 0x07:
XXVIII
handle( 30 );
speed( 100 ,50 );
break;
case 0xe0:
handle( -30 );
speed( 70 ,100 );
break;
case 0x83:
handle( 48 );
speed( 60 ,-10 );
break;
case 0xc1:
handle( -48 );
speed( 10 ,50 );
break;
default:
break;
}
break;
/*****************************************************************/
/*****************************************************************/
default:
/* Return to the standby state for cases that were none of the above. */
XXIX
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 = 0xf3; /*26/9 by zhou*/
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;
XXX
/* ITU1: interrupt every 1 ms */
ITU1_TCR = 0x23;
ITU1_GRA = TIMER1_CYCLE;
ITU1_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 = 0x0B; /*26/9 by zhou*/
ISCR = 0x0c; /*触发方式*/
IER |= 0x0c; /*开中断*/
}
/************************************************************************/
/* ITU0: Interrupt handling */
/************************************************************************/
#pragma interrupt( interrupt_timer0 )
void interrupt_timer0( void )
XXXI
{
ITU0_TSR &= 0xfe; /* Clear the flag */
cnt0++;
cnt1++;
cnt2++;
}
/************************************************************************/
/* ITU1: Interrupt handling */
/************************************************************************/
#pragma interrupt( interrupt_timer1 )
void interrupt_timer1( void )
{
ITU1_TSR &= 0xfe; /* Clear the flag */
cnt_lsec++;
cnt_rsec++;
}
/**************************************************************************/
/*左马达中断计数*/
/**************************************************************************/
#pragma interrupt( l_count )
void l_count( void )
{
ISR &= 0xf7; /* Clear the flag */
this_cnt_lsec=cnt_lsec;
cnt_lsec=0;
XXXII
}
/*************************************************************************/
/*右马达中断计数*/
/*************************************************************************/
#pragma interrupt( r_count )
void r_count( void )
{
ISR &= 0xfb; /* Clear the flag */
this_cnt_rsec=cnt_rsec;
cnt_rsec=0;
}
/************************************************************************/
/* 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 */
/************************************************************************/ XXXIII
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;
XXXIV
}
/************************************************************************/
/* 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(MASK4_4);
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;
XXXV
ret = 0;
b = sensor_inp(MASK4_4);
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 )
XXXVI
{
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 */
/************************************************************************/ XXXVII
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 = (unsigned int)(speed_max * accele_l / 100); /* */
}
XXXVIII
else
{
PBDR |= 0x04; /*设置转动方向为反转*/
accele_l = -accele_l;
ITU3_BRB = (unsigned int)(speed_max * accele_l / 100);
}
/* Right motor */
if( accele_r >= 0 )
{
PBDR &= 0xf7; /*设置转动方向为正转*/
ITU4_BRA = (unsigned int)(speed_max * accele_r / 100);
}
else
{
PBDR |= 0x08; /*设置转动方向为反转*/
accele_r = -accele_r;
ITU4_BRA = (unsigned int)(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( float angle )
{
XXXIX
if(angle > 48)
{
angle = 48;
}
else if(angle < -48)
{
angle = -48;
}
ITU4_BRB = (unsigned int) (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 */
}
XL
return ret;
}
// 初始化PID模块的代码
void pid_init(struct pid_unit *unit, // PID控制模块指针
PID_TYPE p, // 比例系数
PID_TYPE i, // 积分系数
PID_TYPE d) // 微分系数 {
unit->pid_cpid[0] = p;
unit->pid_cpid[1] = i;
unit->pid_cpid[2] = d;
unit->pid_dpid[0] = 0;
unit->pid_dpid[1] = 0;
unit->pid_dpid[2] = 0;
unit->pid_sens[0] = 0;
unit->pid_sens[1] = 0;
}
// PID算法控制函数，通过此函数实现对输入数据序列的PID处理 // 返回值:PID计算所得值
PID_TYPE pid_control(struct pid_unit *unit, // PID控制模块
PID_TYPE input, // 当前输入数据
PID_TYPE time) // 距离上个数据的时间间隔 {
// adjust the FIFO preserving the sensor data
XLI
unit->pid_sens[1] = unit->pid_sens[0];
unit->pid_sens[0] = input;
// calculate each pid variable
unit->pid_dpid[0] = unit->pid_sens[0] * unit->pid_cpid[0];
unit->pid_dpid[1] += unit->pid_sens[0] * unit->pid_cpid[1] * time;
unit->pid_dpid[2] = (unit->pid_sens[0] - unit->pid_sens[1])/ time *
unit->pid_cpid[2];
return unit->pid_dpid[0] +
unit->pid_dpid[1] +
unit->pid_dpid[2];
}
/*赋值lspeed*/
int getlspeed()
{
if(this_cnt_lsec)
{
ltemp=(D*PI*0.05)/(this_cnt_lsec*0.2);
if(ltemp<5)
{
lspeed=ltemp;
}
}
return 1;
}
XLII
/*赋值rspeed*/
int getrspeed()
{
if(this_cnt_rsec)
{
rtemp=(D*PI*0.05)/(this_cnt_rsec*0.2);
if(rtemp<5)
{
rspeed=rtemp;
}
}
return 1;
}
/************************************************************************/
/* end of file */
/************************************************************************/
XLIII
浙公网安备 33021202002483