coolfactor/wiimotionplus
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Import Wii Motion Plus demo code

Browse Source
This commit is contained in:
Gavan Fantom
2010-02-03 00:24:16 +00:00
commit 237b952e89
6 changed files with 492 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
Makefile Normal file
View File

@@ -0,0 +1,19 @@
# Makefile
CFLAGS+= -I/c/SDL-1.2.7/include/SDL -Wall -Werror
LDFLAGS+= -L/c/SDL-1.2.7/lib
LDLIBS+= -lmingw32 -lopengl32 -lglu32 -lSDLmain -mwindows -lSDL
OBJS= gltest.o i2c.o ioport.o
gltest: $(OBJS)
gltest.o: gltest.c i2c.h
i2c.o: i2c.h ioport.h
ioport.c: ioport.h
clean:
rm -f $(OBJS) gltest.exe stdout.txt

225
gltest.c Normal file
View File

@@ -0,0 +1,225 @@
/* gltest.c */
#include <stdlib.h>
#include "SDL.h"
#include "i2c.h"
#include "SDL_opengl.h"
static void setup_opengl( int width, int height )
{
float ratio = (float) width / (float) height;
glShadeModel( GL_SMOOTH );
glCullFace( GL_BACK );
glFrontFace( GL_CCW );
glEnable( GL_CULL_FACE );
glClearColor( 0, 0, 0, 0 );
glViewport( 0, 0, width, height );
glMatrixMode( GL_PROJECTION );
glLoadIdentity( );
gluPerspective( 60.0, ratio, 1.0, 1024.0 );
}
void drawscene(int r, int p, int y)
{
static float roll, pitch, yaw;
roll += (float)r / 1000;
pitch += (float)p / 1000;
yaw += (float)y / 1000;
if (roll > 360.0)
roll -= 360.0;
if (roll < 360.0)
roll += 360.0;
if (pitch > 360.0)
pitch -= 360.0;
if (pitch < 360.0)
pitch += 360.0;
if (yaw > 360.0)
yaw -= 360.0;
if (yaw < 360.0)
yaw += 360.0;
static GLfloat v0[] = { -1.0f, -1.0f, 1.0f };
static GLfloat v1[] = { 1.0f, -1.0f, 1.0f };
static GLfloat v2[] = { 1.0f, 1.0f, 1.0f };
static GLfloat v3[] = { -1.0f, 1.0f, 1.0f };
static GLfloat v4[] = { -1.0f, -1.0f, -1.0f };
static GLfloat v5[] = { 1.0f, -1.0f, -1.0f };
static GLfloat v6[] = { 1.0f, 1.0f, -1.0f };
static GLfloat v7[] = { -1.0f, 1.0f, -1.0f };
static GLubyte red[] = { 255, 0, 0, 255 };
static GLubyte green[] = { 0, 255, 0, 255 };
static GLubyte blue[] = { 0, 0, 255, 255 };
static GLubyte white[] = { 255, 255, 255, 255 };
static GLubyte yellow[] = { 0, 255, 255, 255 };
static GLubyte black[] = { 0, 0, 0, 255 };
static GLubyte orange[] = { 255, 255, 0, 255 };
static GLubyte purple[] = { 255, 0, 255, 0 };
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
/* We don't want to modify the projection matrix. */
glMatrixMode( GL_MODELVIEW );
glLoadIdentity( );
/* Move down the z-axis. */
glTranslatef( (float)0*4, (float)0*4, -5.0 );
/* Apply rotation from our model */
glRotatef( roll, 1.0, 0.0, 0.0 );
glRotatef( pitch, 0.0, 0.0, 1.0 );
glRotatef( yaw, 0.0, -1.0, 0.0 );
/* Send our triangle data to the pipeline. */
glBegin( GL_TRIANGLES );
glColor4ubv( red );
glVertex3fv( v0 );
glColor4ubv( green );
glVertex3fv( v1 );
glColor4ubv( blue );
glVertex3fv( v2 );
glColor4ubv( red );
glVertex3fv( v0 );
glColor4ubv( blue );
glVertex3fv( v2 );
glColor4ubv( white );
glVertex3fv( v3 );
glColor4ubv( green );
glVertex3fv( v1 );
glColor4ubv( black );
glVertex3fv( v5 );
glColor4ubv( orange );
glVertex3fv( v6 );
glColor4ubv( green );
glVertex3fv( v1 );
glColor4ubv( orange );
glVertex3fv( v6 );
glColor4ubv( blue );
glVertex3fv( v2 );
glColor4ubv( black );
glVertex3fv( v5 );
glColor4ubv( yellow );
glVertex3fv( v4 );
glColor4ubv( purple );
glVertex3fv( v7 );
glColor4ubv( black );
glVertex3fv( v5 );
glColor4ubv( purple );
glVertex3fv( v7 );
glColor4ubv( orange );
glVertex3fv( v6 );
glColor4ubv( yellow );
glVertex3fv( v4 );
glColor4ubv( red );
glVertex3fv( v0 );
glColor4ubv( white );
glVertex3fv( v3 );
glColor4ubv( yellow );
glVertex3fv( v4 );
glColor4ubv( white );
glVertex3fv( v3 );
glColor4ubv( purple );
glVertex3fv( v7 );
glColor4ubv( white );
glVertex3fv( v3 );
glColor4ubv( blue );
glVertex3fv( v2 );
glColor4ubv( orange );
glVertex3fv( v6 );
glColor4ubv( white );
glVertex3fv( v3 );
glColor4ubv( orange );
glVertex3fv( v6 );
glColor4ubv( purple );
glVertex3fv( v7 );
glColor4ubv( green );
glVertex3fv( v1 );
glColor4ubv( red );
glVertex3fv( v0 );
glColor4ubv( yellow );
glVertex3fv( v4 );
glColor4ubv( green );
glVertex3fv( v1 );
glColor4ubv( yellow );
glVertex3fv( v4 );
glColor4ubv( black );
glVertex3fv( v5 );
glEnd( );
SDL_GL_SwapBuffers();
}
int main(int argc, char *argv[])
{
SDL_Surface *screen;
int roll, pitch, yaw;
if ( SDL_Init(SDL_INIT_AUDIO|SDL_INIT_VIDEO) < 0 ) {
fprintf(stderr, "Unable to init SDL: %s\n", SDL_GetError());
exit(1);
}
atexit(SDL_Quit);
SDL_GL_SetAttribute( SDL_GL_RED_SIZE, 8 );
SDL_GL_SetAttribute( SDL_GL_GREEN_SIZE, 8 );
SDL_GL_SetAttribute( SDL_GL_BLUE_SIZE, 8 );
SDL_GL_SetAttribute( SDL_GL_DEPTH_SIZE, 32 );
SDL_GL_SetAttribute( SDL_GL_DOUBLEBUFFER, 1 );
//screen = SDL_SetVideoMode(640, 480, 32, SDL_HWSURFACE);
screen = SDL_SetVideoMode(640, 480, 32, SDL_OPENGL);
if ( screen == NULL ) {
fprintf(stderr, "Unable to set 640x480 video: %s\n", SDL_GetError());
exit(1);
}
setup_opengl(640, 480);
initparallel();
setupi2c();
calibrate();
query_device(&roll, &pitch, &yaw);
drawscene(roll, pitch, yaw);
while (1)
{
SDL_Event event;
/* SDL_WaitEvent(&event); */
while (SDL_PollEvent(&event))
{
switch (event.type)
{
case SDL_QUIT:
case SDL_KEYDOWN:
exit(0);
}
}
query_device(&roll, &pitch, &yaw);
drawscene(roll, pitch, yaw);
}
}

205
i2c.c Normal file
View File

@@ -0,0 +1,205 @@
#include <windows.h>
#include <stdio.h>
#include "ioport.h"
/*
* This is a hacked up piece of bit-banging code to query a Wii Motion Plus
* over I2C on the parallel port. It's not polished code by a long way.
* You have been warned.
*/
/* This is the first parallel port on most PCs. */
#define PAR 0x378
#define SCL_MASK 0x40
#define SDA_MASK 0x80
#define SDA_IN_MASK 0x40
#if 0
#define DELAY Sleep(1)
#else
int _delay;
#define DELAY for (_delay = 0; _delay < DELAY_COUNT; _delay++) {}
#endif
/*
* Carefully Calibrated[tm] delay loop.
* This works on my PC, it may not work on yours.
* You have been warned.
*/
#define DELAY_COUNT 10000
#define NCAL 100
int roll0, pitch0, yaw0;
int paroutvalue = SCL_MASK | SDA_MASK;
void initparallel(void)
{
paroutvalue = SCL_MASK | SDA_MASK;
outportb(PAR+2, 0x20);
outportb(PAR+0, paroutvalue);
DELAY;
}
void setscl(int value)
{
/* We have an inverter in the way */
value = !value;
paroutvalue &= ~SCL_MASK;
paroutvalue |= (value ? SCL_MASK : 0);
outportb(PAR+0, paroutvalue);
DELAY;
}
void setsda(int value)
{
/* We have an inverter in the way */
value = !value;
paroutvalue &= ~SDA_MASK;
paroutvalue |= (value ? SDA_MASK : 0);
outportb(PAR+0, paroutvalue);
DELAY;
}
int getsda(void)
{
int value;
value = inportb(PAR+1) & SDA_IN_MASK;
/* We have an inverter in the way */
value = !value;
return (value != 0);
}
void send_start(void)
{
setsda(1);
setscl(1);
setsda(0);
}
void send_stop(void)
{
setsda(0);
setscl(1);
setsda(1);
}
int send_byte(int value)
{
int i;
int ack;
for (i = 0; i < 8; i++) {
setscl(0);
setsda(value & 0x80);
setscl(1);
value = value << 1;
}
setscl(0);
setsda(1);
setscl(1);
ack = !getsda();
setscl(0);
if (!ack) {
send_stop();
printf("ACK not received\n");
return FALSE;
}
return TRUE;
}
int get_byte(int last)
{
int i;
int value = 0;
setsda(1);
for (i = 0; i < 8; i++) {
setscl(0);
setscl(1);
value = value << 1;
value |= (getsda()?1:0);
}
setscl(0);
if (!last)
setsda(0);
setscl(1);
setscl(0);
setsda(1);
return value;
}
void setupi2c(void)
{
send_start();
send_byte((0x53 << 1) + 0); /* Address 0x53, write */
send_byte(0xfe);
send_byte(0x04);
send_stop();
}
void get_readings(int *b)
{
send_start();
send_byte((0x52 << 1) + 0); /* Address 0x52, write */
send_byte(0);
send_stop();
send_start();
send_byte((0x52 << 1) + 1); /* Address 0x52, read */
b[0] = get_byte(0);
b[1] = get_byte(0);
b[2] = get_byte(0);
b[3] = get_byte(0);
b[4] = get_byte(0);
b[5] = get_byte(1);
send_stop();
}
void query_device(int *roll, int *pitch, int *yaw)
{
int b[6];
get_readings(b);
*yaw = ((b[3]>>2)<<8) + b[0] - yaw0;
*pitch = ((b[4]>>2)<<8) + b[1] - pitch0;
*roll = ((b[5]>>2)<<8) + b[2] - roll0;
}
void calibrate(void)
{
int b[6];
int i;
yaw0 = 0;
pitch0 = 0;
roll0 = 0;
printf("Calibrating... ");
for (i = 0; i < NCAL; i++) {
printf("%2d", i);
get_readings(b);
yaw0 += ((b[3]>>2)<<8) + b[0];
pitch0 += ((b[4]>>2)<<8) + b[1];
roll0 += ((b[5]>>2)<<8) + b[2];
printf("\b\b");
}
yaw0 = yaw0 / NCAL;
pitch0 = pitch0 / NCAL;
roll0 = roll0 / NCAL;
printf("done\n");
printf("Calibrated values: %d, %d, %d\n", roll0, pitch0, yaw0);
}

6
i2c.h Normal file
View File

@@ -0,0 +1,6 @@
/* i2c.h */
void initparallel(void);
void setupi2c(void);
void query_device(int *roll, int *pitch, int *yaw);
void calibrate(void);

28
ioport.c Normal file
View File

@@ -0,0 +1,28 @@
#include <windows.h>
#include "ioport.h"
void outport(unsigned int portid, unsigned int value)
{
asm("out %%ax,%%dx" : : "a"(value), "d"(portid));
}
void outportb(unsigned int portid, unsigned char value)
{
asm("out %%al,%%dx" : : "a"(value), "d"(portid));
}
unsigned char inportb(unsigned int portid)
{
unsigned char value;
asm("in %%dx,%%al" : "=a"(value) : "d"(portid));
return value;
}
unsigned int inport(unsigned int portid)
{
int value=0;
asm("in %%dx,%%ax" : "=a"(value) : "a"(value), "d"(portid));
return value;
}

9
ioport.h Normal file
View File

@@ -0,0 +1,9 @@
#ifndef IOPORTH
#define IOPORTH
void outport(unsigned int portid, unsigned int value);
void outportb(unsigned int portid, unsigned char value);
unsigned char inportb(unsigned int portid);
unsigned int inport(unsigned int portid);
#endif