|
|
主函数程序：
#include <stc12c5a.h>
#include <intrins.h>
#include <stdio.h>
#include <math.h>
#include "lcd1602.h"
#include "mpu6050.h"
#include "pid_own.h"
#include "kalman.h"
#include "pwm_motor.h"
#include "spe_pos.h"
int INT_PWM;
unsigned int Init_Time=0,Start_Flag=0;
void main()
{
lcd_init();
InitMPU6050();
PWM_Motor_Init();
INT_Init();
P1M0=1;
P1M1=0;
while(1)
{
if(GYRO_X<0)
{
write_com(0x80);
write_dat('-');
write_dat('0'+(uchar)abs(GYRO_X)/100);
write_dat('0'+(uchar)abs(GYRO_X)%100/10);
write_dat('0'+(uchar)abs(GYRO_X)%10);
}
else
{
write_com(0x80);
write_dat('+');
write_dat('0'+(uchar)GYRO_X/100);
write_dat('0'+(uchar)GYRO_X%100/10);
write_dat('0'+(uchar)GYRO_X%10);
}
if(Angle_End<0)
{
write_com(0x80+0x40);
write_dat('-');
write_dat('0'+(uchar)abs(Angle_End)/100);
write_dat('0'+(uchar)abs(Angle_End)%100/10);
write_dat('0'+(uchar)abs(Angle_End)%10);
}
else
{
write_com(0x80+0x40);
write_dat('+');
write_dat('0'+(uchar)Angle_End/100);
write_dat('0'+(uchar)Angle_End%100/10);
write_dat('0'+(uchar)Angle_End%10);
}
if(speed<0)
{
write_com(0x80+9);
write_dat('-');
write_dat('0'+(uchar)abs(speed)/100);
write_dat('0'+(uchar)abs(speed)%100/10);
write_dat('0'+(uchar)abs(speed)%10);
}
else
{
write_com(0x80+9);
write_dat('+');
write_dat('0'+(uchar)speed/100);
write_dat('0'+(uchar)speed%100/10);
write_dat('0'+(uchar)speed%10);
}
if(position<0)
{
write_com(0x80+0x40+9);
write_dat('-');
write_dat('0'+(uint)abs(position)/10000);
write_dat('0'+(uint)abs(position)%10000/1000);
write_dat('0'+(uint)abs(position)%1000/100);
write_dat('0'+(uint)abs(position)%100/10);
write_dat('0'+(uint)abs(position)%10);
}
else
{
write_com(0x80+0x40+9);
write_dat('+');
write_dat('0'+(uint)abs(position)/10000);
write_dat('0'+(uint)abs(position)%10000/1000);
write_dat('0'+(uint)position%1000/100);
write_dat('0'+(uint)position%100/10);
write_dat('0'+(uint)position%10);
}
if(Start_Flag)
{
INT_PWM = pid_proc(Angle_End,Gyro_End,speed,position);
Motor_Con(-INT_PWM,-INT_PWM);
}
}
}
void timer1() interrupt 3
{
TL1 = 0x00; //定时10MS
TH1 = 0xB8;
if(!Start_Flag)//启动前的延时
{
Init_Time++;
if(Init_Time>=100) Start_Flag=1;
}
if(Start_Flag)
{
Get_Date();
Kalman_Filter(Angel_accY,GYRO_X);
Speed_Position_Get();
speed_mr = speed_ml = 0;
}
}
完整的程序在附件中
|
|
