標題: STC12單片機做平衡車程序 mpu6050+L298N+直流減速電機 [打印本頁]
作者: 生鐵0    時間: 2019-7-31 16:21
標題: STC12單片機做平衡車程序 mpu6050+L298N+直流減速電機
大家參考參考。
mpu6050+L298N+直流減速電機

單片機源程序如下:
  1. #include <stc12c5a.h>
  2. #include <intrins.h>
  3. #include <stdio.h>
  4. #include <math.h>

  6. #include "kalman.h"
  7. #include "lcd1602.h"
  8. #include "mpu6050.h"
  9. #include "pid_own.h"
  10. #include "pwm_motor.h"
  11. #include "spe_pos.h"


  14. int INT_PWM;
  15. unsigned int Init_Time=0,Start_Flag=0;


  18. void main()
  19. {
  20.         lcd_init();
  21.         InitMPU6050();
  22.         PWM_Motor_Init();
  23.         INT_Init();
  24.        
  25.         P1M0=1;
  26.         P1M1=0;
  27.        

  29.         while(1)
  30.         {

  32.                 if(GYRO_X<0)
  33.                 {
  34.                         write_com(0x80);
  35.                         write_dat('-');
  36.                         write_dat('0'+(uchar)abs(GYRO_X)/100);
  37.                         write_dat('0'+(uchar)abs(GYRO_X)%100/10);
  38.                         write_dat('0'+(uchar)abs(GYRO_X)%10);
  39.                 }
  40.                 else
  41.                 {
  42.                         write_com(0x80);
  43.                         write_dat('+');
  44.                         write_dat('0'+(uchar)GYRO_X/100);
  45.                         write_dat('0'+(uchar)GYRO_X%100/10);
  46.                         write_dat('0'+(uchar)GYRO_X%10);
  47.                 }

  49.                 if(Angle_End<0)
  50.                 {
  51.                         write_com(0x80+0x40);
  52.                         write_dat('-');
  53.                         write_dat('0'+(uchar)abs(Angle_End)/100);
  54.                         write_dat('0'+(uchar)abs(Angle_End)%100/10);
  55.                         write_dat('0'+(uchar)abs(Angle_End)%10);
  56.                 }
  57.                 else
  58.                 {
  59.                         write_com(0x80+0x40);
  60.                         write_dat('+');
  61.                         write_dat('0'+(uchar)Angle_End/100);
  62.                         write_dat('0'+(uchar)Angle_End%100/10);
  63.                         write_dat('0'+(uchar)Angle_End%10);
  64.                 }
  65.                
  66.                 if(speed<0)
  67.                 {
  68.                         write_com(0x80+9);
  69.                         write_dat('-');
  70.                         write_dat('0'+(uchar)abs(speed)/100);
  71.                         write_dat('0'+(uchar)abs(speed)%100/10);
  72.                         write_dat('0'+(uchar)abs(speed)%10);
  73.                 }
  74.                 else
  75.                 {
  76.                         write_com(0x80+9);
  77.                         write_dat('+');
  78.                         write_dat('0'+(uchar)speed/100);
  79.                         write_dat('0'+(uchar)speed%100/10);
  80.                         write_dat('0'+(uchar)speed%10);
  81.                 }
  82.                
  83.                 if(position<0)
  84.                 {
  85.                         write_com(0x80+0x40+9);
  86.                         write_dat('-');
  87.                         write_dat('0'+(uint)abs(position)/10000);
  88.                         write_dat('0'+(uint)abs(position)%10000/1000);
  89.                         write_dat('0'+(uint)abs(position)%1000/100);
  90.                         write_dat('0'+(uint)abs(position)%100/10);
  91.                         write_dat('0'+(uint)abs(position)%10);
  92.                 }
  93.                 else
  94.                 {
  95.                         write_com(0x80+0x40+9);
  96.                         write_dat('+');
  97.                         write_dat('0'+(uint)abs(position)/10000);
  98.                         write_dat('0'+(uint)abs(position)%10000/1000);
  99.                         write_dat('0'+(uint)position%1000/100);
  100.                         write_dat('0'+(uint)position%100/10);
  101.                         write_dat('0'+(uint)position%10);
  102.                 }
  103.                
  104.                 if(Start_Flag)
  105.                 {
  106.                         INT_PWM = pid_proc(Angle_End,Gyro_End,speed,position);
  107.                        
  108.                         Motor_Con(-INT_PWM,-INT_PWM);
  109.                        
  110.                 }
  111.         }
  112.        
  113. }

  115. void timer1() interrupt 3
  116. {
  117.         TL1 = 0x00;                    //定時10MS
  118.         TH1 = 0xB8;
  119.        
  120.         if(!Start_Flag)                //啟動前的延時
  121.         {
  122.                 Init_Time++;
  123.                 if(Init_Time>=100) Start_Flag=1;
  124.         }
  125.        
  126.         if(Start_Flag)
  127.         {
  128.                 Get_Date();
  129.                 Kalman_Filter(Angel_accY,GYRO_X);
  130.                 Speed_Position_Get();
  131.                 speed_mr = speed_ml = 0;
  132.         }
  133. }
復(fù)制代碼

所有程序51hei提供下載:
小車平衡STC穩(wěn)定.zip (196.23 KB, 下載次數(shù): 114)


作者: admin    時間: 2019-7-31 22:52
本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1