MPU 6050 VỚI PIC16F877A

[vnantien]

em mới làm cái này mong mọi người chỉ với, e có đọc code trên mạng k hiểu chỗ hàm set_tick va get_tick. mọi người giải thích và cho e xin 2 hàm này với. tks!

#include <16F877A.h>
#device ADC=10
#FUSES NOWDT                    //No Watch Dog Timer
#FUSES NOBROWNOUT               //No brownout reset
#FUSES NOLVP                    //No low voltage prgming, B3(PIC16) or B5(PIC18) used FOR I/O
#use delay(crystal=12MHz)      
//#use i2c(master, sda=PIN_C4, scl=PIN_C3,FORCE_HW)
//#use i2c(master, sda=PIN_E0, scl=PIN_E1)
#define MPU_SDA PIN_E0                            
#define MPU_SCL PIN_E1                      
#use I2C(master, sda=MPU_SDA, scl=MPU_SCL)
//#use TIMER(TIMER=1, TICK=1ms, BITS=16, NOISR)    
#define LCD_RS_PIN PIN_D0                                                  
#define LCD_RW_PIN PIN_D1      
#define LCD_ENABLE_PIN PIN_D2                        
#define LCD_DATA4 PIN_D4                                                        
#define LCD_DATA5 PIN_D5                                  
#define LCD_DATA6 PIN_D6                                                                          
#define LCD_DATA7 PIN_D7                                  
#include <lcd.c>
#include "MPU6050.c"            
#include "math.h"
#include "Kalman.h"
#define RAD_TO_DEG 180/PI                                                                                                                                     

void main()
{
   SIGNED int16 accX,accY, accZ;
   SIGNED int16 gyroX,gyroY,gyroZ;
   DOUBLE accXangle,accYangle;
   DOUBLE gyroXangle,gyroYangle;
   DOUBLE kalAngleX,kalAngleY;          
   UNSIGNED int16 timer;
   lcd_init ();
   printf (LCD_PUTC, "\f MuaLinhKien.Vn
PIC 16/18 Basic Kit");
   delay_ms (1000);
   printf (LCD_PUTC, "\f");
   Mpu6050_Init () ;
   delay_ms (500) ;
   INT8 x;
   x = Mpu6050_Read(MPU6050_RA_WHO_AM_I);
                                                     
   IF (x != 0x68)
   {
      LCD_Gotoxy (2, 0) ;
      printf (LCD_PUTC, "Connection ERR!!!");
      return;
   }                  

 
   WHILE (true)
   {                                                            
      accX = Mpu6050_GetData (MPU6050_RA_ACCEL_XOUT_H);
      accY = Mpu6050_GetData (MPU6050_RA_ACCEL_YOUT_H);
      accZ = Mpu6050_GetData (MPU6050_RA_ACCEL_ZOUT_H);
      gyroX = Mpu6050_GetData(MPU6050_RA_GYRO_XOUT_H);
      gyroY = Mpu6050_GetData(MPU6050_RA_GYRO_YOUT_H);
      gyroZ = Mpu6050_GetData(MPU6050_RA_GYRO_ZOUT_H);
//!      lcd_gotoxy(1,1);                
//!      printf(LCD_PUTC,"G:X%5lu",gyroX);      
//!      lcd_gotoxy(10,1);
//!      printf(LCD_PUTC,"Y%5lu",gyroY);
//!      lcd_gotoxy(1,2);                                            
//!      printf(LCD_PUTC,"Z%5lu",gyroZ);          
//!      delay_ms(100);                  
                                             
//!      lcd_gotoxy(1,1);                
//!      printf(LCD_PUTC,"A:X%5lu",accX);                  
//!      lcd_gotoxy(10,1);                  
//!      printf(LCD_PUTC,"Y%5lu",accY);
//!      lcd_gotoxy(1,2);                    
//!      printf(LCD_PUTC,"Z%5lu",accZ);                  
//!      Delay_ms( 100 );
   
      set_ticks(0);
      accXangle = (atan2 (accY, accZ) + PI) * RAD_TO_DEG;
      DOUBLE gyroXrate = (double) gyroX / 131.0;
      gyroXangle += gyroXrate * ( (DOUBLE) (get_ticks () - timer)  / 1000);
      kalAngleX = kalmanCalculate (accXangle, gyroXrate, (get_ticks()-timer));
      timer = get_ticks ();
      timer=0;
      set_ticks(0);                  
      accYangle = (atan2 (accX, accZ) + PI) * RAD_TO_DEG;
      DOUBLE gyroYrate = (double) gyroY / 131.0;                                                
      gyroYangle += gyroYrate * ( (DOUBLE) (get_ticks () - timer)  / 1000);
      kalAngleY = kalmanCalculate (accYangle, gyroYrate, (get_ticks()-timer));
      timer = get_ticks ();
     
      LCD_Gotoxy (1, 1) ;                              
      printf (LCD_PUTC, "X=%f", kalAngleX);
   
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "       " );
      if(kalAngleX>200)
      {
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "back   " );
      }                                  
      if((150<kalAngleX)&&(kalAngleX<170))        
      {
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "forward" );
      }
      if(kalAngleX<150)      
      {                          
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "nitro  " );
      }
                                       
       LCD_Gotoxy (1, 2) ;              
      printf (LCD_PUTC, "Y=%f", kalAngleY);
       LCD_Gotoxy (10, 2) ;
      printf (LCD_PUTC, "       " );
      if(kalAngleY>200)
      {
      LCD_Gotoxy (10, 2) ;    
      printf (LCD_PUTC, "left  " );
      }
      if(kalAngleY<160)                        
      {
      LCD_Gotoxy (10, 2) ;
      printf (LCD_PUTC, "right " );
      }
      delay_ms(100);
     }              
}

View more random threads:

• cần mua vitme đai ốc bi
• Mua Driver điều khiển động cơ DC( Hà Nội)
• Bộ điều khiển PID mờ cho robot bám theo mép tường ?
• Help. cần hướng dẫn làm robot
• Về lập trình điều xung bằng 89s52
• Chủ đề và luật thi robocon 2014
• Nơi bán động cơ dc có giảm tốc!
• tìm mua động cơ DC servo 24V 60W 3000rpm 1000xung
• Mô phỏng roboot trong Matlab.
• robot leo cầu thang