RoBoard魔人的機器人日誌

2011/11/29

[範例] 讀取wii搖桿

讀取wii搖桿之值


螢幕上顯示的有toy X,Y 為搖桿兩軸
accel X,Y,Z 為加速度三軸
button Z,C 為兩顆按鈕


以下為code


#include "stdio.h"
#include "conio.h"
#define USE_COMMON
#include "roboard.h"
#include "roboard_dll.h"

const unsigned char wii_addr = 0x52;
int joy_x_axis;
int joy_y_axis;
int accel_x_axis;
int accel_y_axis;
int accel_z_axis;
int z_button;
int c_button;
void wii_data(unsigned char*);
int main(){
 roboio_SetRBVer(RB_110);
 unsigned char buf[6];

    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_FAST, 400000L);
 i2c0master_StartN(wii_addr, I2C_WRITE, 2);
 i2c0master_WriteN(0x40);
 i2c0master_WriteN(0x00);

 while(!kbhit()){

  i2c0master_StartN(wii_addr, I2C_WRITE, 1);
  i2c0master_WriteN(0x00);
  delay_ms(5);
  i2c_Receive(wii_addr,buf,6);
  for(int i = 0; i < 6; i++)
   buf[i] = (buf[i] ^ 0x17) + 0x17;
  wii_data(buf);
  printf("toy X:%d\ttoy Y:%d\n",joy_x_axis,joy_y_axis);
  printf("accel X:%d\taccel Y:%d\taccel Z:%d\n", accel_x_axis, accel_y_axis, accel_z_axis);
  printf("button Z:%d\tbutton C:%d\n",z_button,c_button);
  system("cls");
 }
 i2c_Close();
 return 0;
}
void wii_data(unsigned char *buf){
joy_x_axis = buf[0];
joy_y_axis = buf[1];
accel_x_axis = buf[2] << 2;
accel_y_axis = buf[3] << 2;
accel_z_axis = buf[4] << 2;
z_button = 1;
c_button = 1;
if ((buf[5] >> 0) & 1) z_button = 0;
if ((buf[5] >> 1) & 1) c_button = 0;
if ((buf[5] >> 2) & 1) accel_x_axis += 2;
if ((buf[5] >> 3) & 1) accel_x_axis += 1;
if ((buf[5] >> 4) & 1) accel_y_axis += 2;
if ((buf[5] >> 5) & 1) accel_y_axis += 1;
if ((buf[5] >> 6) & 1) accel_z_axis += 2;
if ((buf[5] >> 7) & 1) accel_z_axis += 1;
}
Share:

2 則留言:

技術提供:Blogger.

追蹤者