PIC Vietnam

PIC Vietnam (http://www.picvietnam.com/forum/index.php)
-   PIC - Thiết kế và Ứng dụng (http://www.picvietnam.com/forum/forumdisplay.php?f=23)
-   -   Cần giúp MPU6050 với PIC (http://www.picvietnam.com/forum/showthread.php?t=35703)

pnanhbk 14-11-2016 03:12 PM

Cần giúp MPU6050 với PIC
 
Em đang làm đề tài sử dụng cảm biến gia tốc MPU6050 dùng PIC, em có tham khảo code của 1 anh tên Son Bui trên 1 diễn đàn, em không hiểu hàm "get_ticks()" trong code, em mới học nên k biết mong mọi người chỉ e với ạ. cảm ơn tất cả mọi người !

#include <ex_usb_common.h>
#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 RAD_TO_DEG 180/PI
#define LCD_ENABLE_PIN PIN_D2
#define LCD_RS_PIN PIN_D0
#define LCD_RW_PIN PIN_D1
#define LCD_DATA4 PIN_D4
#define LCD_DATA5 PIN_D5
#define LCD_DATA6 PIN_D6
#define LCD_DATA7 PIN_D7
#include <lcd.c>
/////////////////////////////////////////////////////////////////////////////
//
// Include the CCS USB Libraries. See the comments at the top of these
// files for more information
//
/////////////////////////////////////////////////////////////////////////////
#if __USB_PIC_PERIF__
#if defined(__PCM__)
#include <pic16f_usb.h> //Microchip PIC16Fxxxx hardware layer for usb.c
#elif defined(__PCH__)
#include <pic18_usb.h> //Microchip PIC18Fxx5x hardware layer for usb.c
#elif defined(__PCD__)
#include <pic24_usb.h> //Microchip PIC18Fxx5x hardware layer for usb.c
#endif
#else
#include <usbn960x.h> //National 960x hardware layer for usb.c
#endif
#include <usb_desc_keyboard.h> //USB Configuration and Device descriptors for this UBS device
#include <usb.c> //handles usb setup tokens and get descriptor reports
#include "MPU6050.c"
#include "math.h"
#include "Kalman.h"

typedef enum
{
KB_MODIFIER_LEFT_CTRL = 1,
KB_MODIFIER_LEFT_SHIFT = 2,
KB_MODIFIER_LEFT_ALT = 4,
KB_MODIFIER_LEFT_GUI = 8,
KB_MODIFIER_RIGHT_CTRL = 16,
KB_MODIFIER_RIGHT_SHIFT = 32,
KB_MODIFIER_RIGHT_ALT = 64,
KB_MODIFIER_RIGHT_GUI = 128
} kb_modifier_t;

typedef enum
{
USB_KBD_SCANCODE_ESC = 0x29,
USB_KBD_SCANCODE_CAPS_LOC = 0x39,
USB_KBD_SCANCODE_F1 = 0x3A, //F2 is 0x3B, F3 is 0x3C, etc. this is valid up to F12
USB_KBD_SCANCODE_PRTSC = 0x46,
USB_KBD_SCANCODE_SCROLL_LOCK = 0x47,
USB_KBD_SCANCODE_PAUSE = 0x48,
USB_KBD_SCANCODE_INS = 0x49,
USB_KBD_SCANCODE_HOME = 0x4A,
USB_KBD_SCANCODE_PG_UP = 0x4B,
USB_KBD_SCANCODE_DEL = 0x4C,
USB_KBD_SCANCODE_END = 0x4D,
USB_KBD_SCANCODE_PG_DN = 0x4E,
USB_KBD_SCANCODE_RIGHT = 0x4F,
USB_KBD_SCANCODE_LEFT = 0x50,
USB_KBD_SCANCODE_DOWN = 0x51,
USB_KBD_SCANCODE_UP = 0x52,
USB_KBD_SCANCODE_NUM_LOCK = 0x53,
USB_KBD_SCANCODE_WIN_MENU = 0x65,
USB_KBD_SCANCODE_F13 = 0x68, //F14 is 0x69, F15 is 0x6A, etc. this is valid up to F24
USB_KBD_SCANCODE_HELP = 0x75,
USB_KBD_SCANCODE_UNDO = 0x7A,
USB_KBD_SCANCODE_CUT = 0x7B,
USB_KBD_SCANCODE_COPY = 0x7C,
USB_KBD_SCANCODE_PASTE = 0x7D,
USB_KBD_SCANCODE_MUTE = 0x7F,
USB_KBD_SCANCODE_VOL_UP = 0x80,
USB_KBD_SCANCODE_VOL_DOWN = 0x81
} kb_scancode_t;

signed int16 accX,accY, accZ;
SIGNED int16 gyroX,gyroY,gyroZ;
DOUBLE accXangle,accYangle;
DOUBLE gyroXangle,gyroYangle;
DOUBLE kalAngleX,kalAngleY;
UNSIGNED int16 timer;
UNSIGNED int16 key=0x00,key2=0x00;


unsigned int16 mpu_6050_task(void)
{

//chuong trinh

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);
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 ();

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);
key=0x00;
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, " " );
if(kalAngleX>200)
{
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, "back " );
key= 0x51;//Down
}
if((150<kalAngleX)&&(kalAngleX<170))
{
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, "forward" );
key= 0x52;//Up
}
if(kalAngleX<150)
{
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, "nitro " );
key= 0x2C;//Space

}

LCD_Gotoxy (1, 2) ;
printf (LCD_PUTC, "Y=%f", kalAngleY);
LCD_Gotoxy (10, 2) ;
printf (LCD_PUTC, " " );
key2=0x00;
if(kalAngleY>200)
{
LCD_Gotoxy (10, 2) ;
printf (LCD_PUTC, "left " );
key2= 0x50;

}
if(kalAngleY<160)
{
LCD_Gotoxy (10, 2) ;
printf (LCD_PUTC, "right " );
key2 = 0x4F;
}
delay_ms(50); //Khong co la se treo
return key;


}
//Het chuong trinh


void usb_debug_task(void)
{
static int8 last_connected;
static int8 last_enumerated;
int8 new_connected;
int8 new_enumerated;

new_connected=usb_attached();
new_enumerated=usb_enumerated();

if (new_connected)
LED_ON(LED2);
else
LED_OFF(LED2);

if (new_enumerated)
{
LED_ON(LED1);
LED_ON(LED2);
}
else
{
LED_OFF(LED1);
LED_OFF(LED2);
}
/*
if (new_connected && !last_connected)
printf (LCD_PUTC, "\fUSB connected\nwaiting for enumaration...");
if (!new_connected && last_connected)
printf (LCD_PUTC, "\fUSB disconnected\nwaiting for connection...");
if (new_enumerated && !last_enumerated)
printf (LCD_PUTC, "\fUSB enumerated\nby PC/HOST");
if (!new_enumerated && last_enumerated)
printf (LCD_PUTC, "\fUSB unenumerated by PC/HOST\nwaiting for enumeration...");
*/
last_connected=new_connected;
last_enumerated=new_enumerated;
}




/////////////////////////////////////////////////////////////////////////////
//
// usb_keyboard_task()
//
// Sends a packet of keyboard data. The protocol was specified in the HID
// report descriptor (see usb_desc_kbmouse.h), and is:
// tx_msg[0] = modifier (see kb_modifier_t)
// tx_msg[1] = const 0
// tx_msg[2:6] = an array of held down keys. a=4, b=5, etc.
// if msg[2:7]={0} then no keys are held down
//
// rx_msg[0] = 5bit bitmap of led status
//
/////////////////////////////////////////////////////////////////////////////
void usb_keyboard_task(void)
{
unsigned int8 tx_msg[7];
unsigned int8 leds;
unsigned int16 scancode;

if (usb_tbe(1))
{
memset(tx_msg, 0x00, sizeof(tx_msg));

//scancode =mpu_6050_task();
//if(scancode!=0x00)
mpu_6050_task();
if(key!=0x00)
{
tx_msg[2] =key;//scancode;
}
if(key2!=0x00)
{
tx_msg[3] =key2;//scancode;
}

usb_put_packet(1, tx_msg, sizeof(tx_msg), USB_DTS_TOGGLE);


}

//receive NUM LOCK, CAPS LOCK, etc LED status from PC.
//we won't do anything with it.
if (usb_kbhit(1))
{
usb_get_packet(1, &leds, 1);
}
}

void main(void)
{

lcd_init ();
printf (LCD_PUTC, "\f MuaLinhKien.Vn \nPIC 16/18 Basic Kit");
delay_ms(1000);
HW_INIT();

LED_ON(LED1);
LED_OFF(LED2);
LED_OFF(LED3);

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;
}
usb_init_cs();
printf (LCD_PUTC, "\f");
while (TRUE)
{
usb_task();
usb_debug_task();


if (usb_enumerated())
{
usb_keyboard_task();
}
}

}


Múi giờ GMT. Hiện tại là 05:48 PM.

Tên diễn đàn: vBulletin Version 3.8.11
Được sáng lập bởi Đoàn Hiệp.
Copyright © PIC Vietnam