From 237b952e8927ef711a957e38a352c425af5eb93b Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Wed, 3 Feb 2010 00:24:16 +0000 Subject: [PATCH] Import Wii Motion Plus demo code --- Makefile | 19 +++++ gltest.c | 225 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ i2c.c | 205 ++++++++++++++++++++++++++++++++++++++++++++++++++ i2c.h | 6 ++ ioport.c | 28 +++++++ ioport.h | 9 +++ 6 files changed, 492 insertions(+) create mode 100644 Makefile create mode 100644 gltest.c create mode 100644 i2c.c create mode 100644 i2c.h create mode 100644 ioport.c create mode 100644 ioport.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..644768d --- /dev/null +++ b/Makefile @@ -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 + diff --git a/gltest.c b/gltest.c new file mode 100644 index 0000000..af4ce8a --- /dev/null +++ b/gltest.c @@ -0,0 +1,225 @@ +/* gltest.c */ + +#include +#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); + } + +} diff --git a/i2c.c b/i2c.c new file mode 100644 index 0000000..e1a1496 --- /dev/null +++ b/i2c.c @@ -0,0 +1,205 @@ +#include +#include +#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); +} diff --git a/i2c.h b/i2c.h new file mode 100644 index 0000000..cc7ae86 --- /dev/null +++ b/i2c.h @@ -0,0 +1,6 @@ +/* i2c.h */ + +void initparallel(void); +void setupi2c(void); +void query_device(int *roll, int *pitch, int *yaw); +void calibrate(void); diff --git a/ioport.c b/ioport.c new file mode 100644 index 0000000..4a8a032 --- /dev/null +++ b/ioport.c @@ -0,0 +1,28 @@ +#include +#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; +} + diff --git a/ioport.h b/ioport.h new file mode 100644 index 0000000..7029852 --- /dev/null +++ b/ioport.h @@ -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