#include?“STC12C5A60S2.h“
#include?“IIC.h“
#include?“kaermanlvbo.h“
#include?“MPU6050.h“
#include?“DIANJI.h“
#define?uint?unsigned?int
#define?uchar?unsigned?char

int???speed_mr;		?//右電機轉速
int???speed_ml;		?//左電機轉速
int???PWM_R;?????????//右輪PWM值計算
int???PWM_L;?????????//左輪PWM值計算
float?PWM;???????????//綜合PWM計算
float?PWMI;			?//PWM積分值

float?speed_r_l;	//電機轉速
float?speed;????????//電機轉速濾波
float?position;	????//位移
uchar?t=0;

void?delay（uint?z）
{
	uint?xy;
	for（x=z;x>0;x--）
		for（y=110;y>0;y--）;
}

void?Init_Timer1（void）	//10毫秒@12MHz100Hz刷新頻率
{
	AUXR?&=?0xBF;		//定時器時鐘12T模式
	TMOD&=0x0f;
	TMOD|=0x10;			//設置定時器模式
	TL1?=?0xF0;		//設置定時初值
	TH1?=?0xD8;		//設置定時初值
	TF1?=?0;		????//清除TF1標志
	TR1?=?1;		????//定時器1開始計時
}

void?Init_Interr（void）	?
{
	EA?=?1;?????//開總中斷
????EX0?=?1;????//開外部中斷INT0
????EX1?=?1;????//開外部中斷INT1
????IT0?=?1;????//下降沿觸發
????IT1?=?1;????//下降沿觸發
	ET1?=?1;????//開定時器1中斷
}

void?Psn_Calcu（void）	?		?//電機轉速和位移值計算
{
	
	speed_r_l?=（speed_mr?+?speed_ml）*0.5;
	speed?*=?0.7;		??????????????????//車輪速度濾波
	speed?+=?speed_r_l*0.3;	
	position?+=?speed;	??????????????????//積分得到位移
	if（position<-6000）?position?=?-6000;
	if（position>?6000）?position?=??6000;	?
}

static?float?code?Kp??=?9;???????//PID參數
static?float?code?Kd??=?2.6;	????//PID參數
static?float?code?Kpn?=?0.01;??????//PID參數
static?float?code?Ksp?=?2.0;	????//PID參數

void?PWM_Calcu（void）	?			???//電機PWM值計算
{
????
	if（angle<-40||angle>40）???????????????//角度過大，關閉電機?
	{??
	??CCAP0H?=?0;
??????CCAP1H?=?0;
	??return;
	}
	PWM??=?Kp*angle?+?Kd*gyro_y;??????????//PID：MPU6050角速度和角度
	PWM?+=?Kpn*position?+?Ksp*speed;??????//PID：電機速度和位置
	PWM_R?=?PWM;
	PWM_L?=?PWM;
	pwm_dianji（PWM_LPWM_R）;??
}

void?main（）
{?

	delay（500）;	???//上電延時
	init_pwm（）;	???????//PWM初始化
????init_timer0（）;?????//初始化定時器0，作為PWM時鐘源
	Init_Timer1（）;?????//初始化定時器1
	Init_Interr（）;?????//中斷初始化
	init_dianji（）;	???//電機控制初始化
	init_mpu6050（）;
	delay（500）;?	???

	while（1）
	{
	??

	}
}

//********timer1中斷***********************

void?Timer1_Update（void）?interrupt?3
{
??
	TL1?=?0xF0;		//設置定時初值?10ms
	TH1?=?0xD8;		//設置定時初值

	angle_calcu（）;??????????????????//傾角計算
	Psn_Calcu（）;????????????????????//電機位移計算
	PWM_Calcu（）;????????????????????//計算PWM值
	???
	speed_mr?=?speed_ml?=?0;	?
}?


//********右電機中斷***********************

void?INT_L（void）?interrupt?0
{

???if（SPDL?==?1）??{?speed_ml++;?}		?//左電機前進
???else		??????{?speed_ml--;?}		?//左電機后退

?}?


//********左電機中斷***********************

void?INT_R（void）?interrupt?2
{

???if（SPDR?==?1）??{?speed_mr++;?}		?//右電機前進
???else		??????{?speed_mr--;?}		?//右電機后退

?}?			???
	

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-01-19?18:22??小車平衡總程序\
?????文件?????????647??2015-06-13?15:51??小車平衡總程序\DIANJI.h
?????文件?????????667??2015-05-27?20:55??小車平衡總程序\DINGYI.h
?????文件????????1632??2015-05-31?11:17??小車平衡總程序\IIC.h
?????文件????????1186??2015-05-27?19:56??小車平衡總程序\LCD1206.h
?????文件????????1201??2015-05-31?15:20??小車平衡總程序\MPU6050.h
?????文件?????????499??2015-06-07?14:11??小車平衡總程序\PWM.h
?????文件???????17735??2015-05-20?15:19??小車平衡總程序\STC12C5A60S2.h
?????文件?????????574??2015-05-31?08:18??小車平衡總程序\init.h
?????文件????????1551??2015-06-13?15:48??小車平衡總程序\kaermanlvbo.h
?????文件???????25630??2015-06-13?15:51??小車平衡總程序\小車平衡總程序
?????文件????????6485??2015-06-13?15:51??小車平衡總程序\小車平衡總程序.LST
?????文件???????28326??2015-06-13?15:51??小車平衡總程序\小車平衡總程序.M51
?????文件???????30316??2015-06-13?15:51??小車平衡總程序\小車平衡總程序.OBJ
?????文件????????2948??2015-06-13?15:48??小車平衡總程序\小車平衡總程序.c
?????文件???????10713??2015-06-13?15:51??小車平衡總程序\小車平衡總程序.hex
?????文件??????????60??2015-06-13?15:51??小車平衡總程序\小車平衡總程序.lnp
?????文件???????????0??2016-01-19?18:22??小車平衡總程序\小車平衡總程序.plg
?????文件???????74516??2016-01-19?18:22??小車平衡總程序\小車平衡總程序.uvgui.huahui
?????文件???????74338??2015-05-31?08:49??小車平衡總程序\小車平衡總程序.uvgui_huahui.bak
?????文件????????8358??2016-01-19?18:22??小車平衡總程序\小車平衡總程序.uvopt
?????文件???????14495??2016-01-19?18:22??小車平衡總程序\小車平衡總程序.uvproj
?????文件???????58852??2015-06-13?15:51??小車平衡總程序\小車平衡總程序_uvopt.bak
?????文件???????14491??2015-05-31?08:20??小車平衡總程序\小車平衡總程序_uvproj.bak