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

Import Wii Motion Plus demo code

master
Gavan Fantom 14 years ago
commit
237b952e89
6 changed files with 492 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 19
      Makefile
  2. 225
      gltest.c
  3. 205
      i2c.c
  4. 6
      i2c.h
  5. 28
      ioport.c
  6. 9
      ioport.h

19
Makefile
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Write Preview
Loading…
Cancel
Save