CCS MPU6050

main.c

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

  }