//////////相關(guān)內(nèi)容可查閱計算機硬件技術(shù)基礎、課件、自動控制理論與單片機等書籍/////////////////

///////////////////////////////////程序基礎//////////////////////////////////////////////////
#pragma small
#include<reg51f.h> ////包括51系列單片機的基本定義
#include<intrins.h>////
#include<math.h>
#include<absacc.h> ////擴展板必備
typedef unsigned char uchar;
typedef unsigned int uint;

///////////////////電路設定////////////////////////
#define BASE1 0x8100////擴展片選信號CS1（觸動開關(guān)擴展）
#define BASE2 0x8200////擴展片選信號CS2（雙頭檢測板1）
#define BASE3 0x8300////擴展片選信號CS3 (雙頭檢測板2)
#define BASE5 0x8500////發(fā)光二極管陣列
#define BASE6 0x8600////CS_P0IN0
#define BASE7 0x8700////CS_P0OUT0
#define BASE8 0x8800////CS_P0IN1
#define BASE9 0x8900////CS_P0OUT1

sbit dir1=P1^3; ////鑒向信號，實質(zhì)為反饋標志
sbit dir2=P1^4;
sbit en=P1^0;   ////電橋使能信號
sbit ddir1=P1^1;////左輪方向標志。
sbit ddir2=P1^2;////右輪方向標志。

//////////////////////基本參數(shù)//////////////////////////////
#define PI       3.1415926
#define D        0.122     ////輪子直徑
#define DISTANCE 0.43      ////L輪子間距
#define PANE     1000      ////碼盤線數(shù)
#define PRESPEED 9000      ////原始最高速度（單位：轉(zhuǎn)/分）
#define RESPEED  300       ////減速最高速度（單位：轉(zhuǎn)/分）
#define RATE     30        ////減速比（原始最高速度/減速最高速度）
#define CONST    0.3       ////（ 減速最高速度與碼盤線數(shù)之比（300（轉(zhuǎn)/分）/1000=0.3），可任意）
#define RIGHT    1
#define LEFT     0
#define LINE     156546    ////走1米兩個輪子的碼盤線數(shù)（計算公式：2*（1/（PI*D））*RATE*PANE）
#define TURN     590       ////轉(zhuǎn)1度兩個輪子的碼盤線數(shù)（計算公式：（（DISTANCE/D）*RATE*PANE）/180）

/////////////////////////控制用變量//////////////////////////////
struct discount
{
	int uk1;   ////上次輸出值
	int videal;////理想速度（單位：轉(zhuǎn)/分）
	int ek;    ////本次誤差
	int ek1;   ////上次誤差
	int ek2;   ////前次誤差
};

/////////////////////////////////////////////////////////////////////////////////////////
data int ukx,uky;              ////實際輸出計數(shù)值
data struct discount sm2,sm1;  ////左右輪結(jié)構(gòu)體變量
data uchar countl,counth;      ////計數(shù)值高低位
data long temp;                ////每次中斷時碼盤線數(shù)（最大值為1500：（（PRESPEED/60）*中斷周期）/1000）*PANE）
data long temp1=0,temp2=0;     ////左右輪總的碼盤線數(shù)
data long due;                 ////經(jīng)PID調(diào)節(jié)后的偏差 
data int uk;                   ////本次輸出值
data int kp1,ki1,kd1;          ////PID參數(shù)
data int kp2,ki2,kd2;
data uchar prestate=0x14,state;////巡線狀態(tài)
data int dv;                   ////速度調(diào)節(jié)量
data int flag=0;               ////巡線開關(guān)
data uchar keystate=0x3f;      ////碰撞開關(guān)狀態(tài)

//////////////////////結(jié)構(gòu)體初始化//////////////////////////
void initstruct(struct discount *p)
{
	p->ek2=0;
	p->ek1=0;
	p->ek=0;
	p->uk1=0;
}	

///////////////////////延時////////////////////////
void delay(uchar i)              ////以0.1秒為基本單位
{
	uchar j,m;
    int l;
	for(j=0;j<i;j++)
	{
		for(l=0;l<100;l++)
		{
			for(m=0;m<250;m++)
			{}
		}
	}
}

///////////////////////電機1速度輸出////////////////////
void motor1(int speed1)////PCA模塊2
{
	if(speed1>=0)      ////正轉(zhuǎn)
		ddir1=0;
	else
	{
		ddir1=1;       ////反轉(zhuǎn)
		speed1=-speed1;
	}
	CCAP2L=speed1;     ////轉(zhuǎn)速，范圍（-128，127）
	CCAP2H=speed1;
	CCAPM2=0x42;       ////8位PWM
}

///////////////////////電機2速度輸出////////////////////
void motor2(int speed2)////PCA模塊3
{
    if(speed2>=0)
		ddir2=1;       ////正轉(zhuǎn)
	else
	{
		ddir2=0;       ////反轉(zhuǎn)
		speed2=-speed2;
	}
	CCAP3L=speed2;     ////轉(zhuǎn)速，范圍（-128，127