STC12C5A60S2單片機(jī)兩輪平衡小車程序源碼

ID:367219 · 發(fā)表于 2020-4-6 09:33

主函數(shù)程序：

#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;
}
}
完整的程序在附件中

ID:1 · 發(fā)表于 2020-4-7 21:31

本帖需要重新編輯補(bǔ)全電路原理圖,源碼,詳細(xì)說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)

帳號		自動登錄	找回密碼
密碼			立即注冊

STC12C5A60S2單片機(jī)兩輪平衡小車程序源碼

相關(guān)帖子