Appendix A: Code

A.1. Device Software

A more manageable listing (as well as the project files) for the device code are available here.

A.1.1 Project.h

// UINT type definition

#ifndef _UINT_DEF_

#define _UINT_DEF_

typedef unsigned int UINT;

#endif /* _UINT_DEF_ */

// BYTE type definition

#ifndef _BYTE_DEF_

#define _BYTE_DEF_

typedef unsigned char BYTE;

#endif /* _BYTE_DEF_ */

// Global Definitions

#define NUM_CHANNELS 6

#define DATA_PACKET_LENGTH 10

//RXC ISR variables

unsigned char r_index; //current string index

unsigned char r_buffer[16]; //input string

unsigned char r_ready; //flag for receive done

unsigned char r_char; //current character

//TX empty ISR variables for getting input

unsigned char t_index; //current string index

unsigned char t_buffer[DATA_PACKET_LENGTH]; //output string

unsigned char t_ready; //flag for transmit done

unsigned char t_char; //current character

// Data channel vector

UINT data[NUM_CHANNELS];

BYTE time0; // sampling time counter

BYTE time1; // transmission time counter

// ADMUX values for each channel selected

flash BYTE muxvals[ NUM_CHANNELS ] = { 0b01100000, 0b01100001, 0b01100010,

0b01100011, 0b01100101, 0b01100110 };

// Function protoytes

void Port_Init(void);

void Timer0_Init(void);

void ACD0_Init(void);

void UART_Init(void);

void send_data(void);

void puts_int(void);

void gets_int(void);

A.1.2 Project.c

#include <Mega32.h>

#include <stdio.h> // sprintf amungst other definitions

#include "project.h"

/*********************************************************/

//UART character-ready ISR

interrupt [USART_RXC] void uart_rec(void){

r_char=UDR; //get a char

//UDR=r_char; //then print it

//build the input string

if (r_char != '\r')

r_buffer[r_index++]=r_char;

else{

putchar('\n'); //use putchar to avoid overwrite

r_buffer[r_index]=0x00; //zero terminate

r_ready=1; //signal cmd processor

UCSRB.7=0; //stop rec ISR

}

}

/**********************************************************/

//UART xmit-empty ISR

interrupt [USART_DRE] void uart_send(void){

t_char = t_buffer[++t_index];

if(t_char == 0){

UCSRB.5=0; //kill isr

t_ready=1; //transmit done

}

else

UDR = t_char ; //send the char

}

/**********************************************************/

// Timer 0 compare ISR

interrupt [TIM0_COMP] void timer0_compare(void){

// increment .5ms time variables

time0++;

time1++;

}

/**********************************************************/

//Entry point and task scheduler loop

void main(void){

// temp vars

BYTE i, prev_time;

UINT temp;

// set timing variables to zero

time0 = 0;

time1 = 0;

Port_Init(); // set up port for proper i/o

Timer0_Init(); // set up timer 0

ACD0_Init(); // set up ADC and start first conversion

UART_Init(); // set up UART

PORTD.7 = 0; // turn LED on

// enable interrupts

#asm

sei

#endasm

// spin forever

while(1){

// 2.5 ms count tick

if( time0 == 5 ){

time0 = 0;

// sample each channel continuously

for( i=0; i<NUM_CHANNELS; i++ ){

// change channel

ADMUX = muxvals[i];

// start new conversion

ADCSRA.6=1;

// poll for completion

while( ADCSRA.6 == 1 );

// grab data

//data[i] += ADCH;

temp = (ADCL>>6);

temp |= ( (UINT)ADCH ) << 2;

data[i] += temp;

}

}

// 10 ms count tick, send the data

if( time1 == 20 ){

time1 = 0;

send_data();

data[0] = 0;

data[1] = 0;

data[2] = 0;

data[3] = 0;

data[4] = 0;

data[5] = 0;

}

}// end while(1)

}

//--------------------------------------------------------

// SEND_DATA

//--------------------------------------------------------

// send x,y,z data out to UART

void send_data(){

// temp variables

BYTE i, temp;

// divide the data by 4 (>>2)

for( i=0; i<NUM_CHANNELS; i++ )

data[i] = data[i]>>2;

// build the data packet

t_buffer[0] = 0x01;

t_buffer[1] = (BYTE)(data[0]&0x00FF); // data[0] lb -- x1

t_buffer[2] = (BYTE)(data[1]&0x00FF); // data[1] lb -- y1

t_buffer[3] = (BYTE)(data[2]&0x00FF); // data[2] lb -- z1

t_buffer[4] = (BYTE)(data[3]&0x00FF); // data[0] lb -- x2

t_buffer[5] = (BYTE)(data[4]&0x00FF); // data[1] lb -- y2

t_buffer[6] = (BYTE)(data[5]&0x00FF); // data[2] lb -- z2

temp = (data[0]>>2)&0b11000000;

temp |= (data[1]>>4)&0b00110000;

temp |= (data[2]>>6)&0b00001100;

temp |= (data[3]>>8)&0b00000011;

t_buffer[7] = temp;

temp = (data[4]>>2)&0b11000000;

temp |= (data[5]>>4)&0b00110000;

t_buffer[8] = temp;

t_buffer[9] = 0x00;

// send the data

puts_int();

}

//---------------------------------------------------------

// ADC0_Init

//---------------------------------------------------------

// Set up ADC with right adjusted values

// Start first conversion

void ACD0_Init(void){

ADMUX = 0b01100000; // using AVCC as reference

// /w left adjusted values

// looking @ ADC0

ADCSRA = 0b10000111; // enable ADC with interrupt on

// auto trigger off, interrupt enable

// start first conversion

ADCSRA.6 = 1;

}

//---------------------------------------------------------

// Port_Init

//---------------------------------------------------------

// Set up I/O PORTS:

// PORTA: input for Analog Signals

void Port_Init(void){

DDRA = 0x00; // port A as an input

PORTA = 0xff; // turn pull-ups on

// port C,D as digital output

DDRC = 0xff;

DDRD = 0xff;

}

//---------------------------------------------------------

// Timer0_Init

//---------------------------------------------------------

// Set up timer 0 with interrupt ticks @ .5ms

void Timer0_Init(void){

TCCR0 = 0b00001011; // clock source @ /64 internal clock

OCR0 = 125; // compare value of 125

TIMSK = 0x02; // enable compare match

}

//---------------------------------------------------------

// UART0_Init

//---------------------------------------------------------

// Set up Uart @ 9600 baud

void UART_Init(void){

UCSRB = 0x18 ;

UBRRL = 103 ;

//init the task timers

r_ready=0;

t_ready=1;

}

//**********************************************************

// -- non-blocking keyboard check initializes ISR-driven

// receive. This routine merely sets up the ISR, which then

//does all the work of getting a command.

void gets_int(void){

r_ready=0;

r_index=0;

UCSRB.7=1; // enables UART_RXC interrupt

}

//**********************************************************

// -- nonblocking print: initializes ISR-driven

// transmit. This routine merely sets up the ISR, then

//send one character, The ISR does all the work.

void puts_int(void){

t_ready=0;

t_index=0;

if (t_buffer[0]>0){

putchar(t_buffer[0]);

UCSRB.5=1; // enables UART_DRE interrupt

}

}

A.2. Application Software

A more manageable listing of the code (including the visual studio project files) are available here.

A.2.1: Main.cpp

#define GAMEX_MAIN

#include <windows.h>

#include <process.h>

#include <stdio.h>

#include "gamex.hpp"

#include "LogFile.hpp"

#include "Console.hpp"

#include "Graph.hpp"

#include "Button.hpp"

#include "SerialPort.hpp"

#include "Point.hpp"

#include "Elbow.hpp"

#include "Wrist.hpp"

#include "Body.hpp"

#define PI 3.14159f

int width = 512;

int height = 768;

char buffer[128];

// Camera for 3D projections

CameraX cam;

Vector3DF view;

Vector3DF axisView;

char txt_x[] = "x";

char txt_y[] = "y";

char txt_z[] = "z";

// GUI

HWND hWnd;

Console *console;

Graph *accGraph[3], *velGraph[3], *posGraph[3], *angGraph[3];

bool showAllGraphs = false, graphsDisabled = true;

Button *startLogButton, *saveLogButton, *runLogButton, *resetButton;

// Motion Capture

Wrist *wrist;

Elbow *elbow;

Point *shoulder;

Body *body;

// Serial Port Stuff

SerialPort serialPort;

BYTE ReadBuffer[255];

DWORD BytesRead;

HANDLE hDataMutex;

bool dataChanged = true, dataChanged_c = true;

Vector3DF accOne;

Vector3DF accTwo;

UINT b[9] = {0,0,0,0,0,0,0,0,0};

int b_ptr = 0;

// File Logging

LogFile logFile;

bool logSerialToFile = false, readFromLogFile = false;

bool doneReadingLog = false;

OPENFILENAME ofn;

char szFileName[128];

BYTE readBuf[128];

void GameInit (void)

{

// ------------- GAMEX BEGIN ---------------

GameX.Initialize ("Motion Capture", RUN_NOCONFIRMQUIT | VIDEO_16BIT

| VIDEO_WINDOWED | RUN_BACKGROUND | RUN_AUTOSHOWINFO, 1280, 768);

view.x = 0; view.y = 0.5f; view.z = 250;

axisView.x = 0; axisView.y = 0; axisView.z = 60;

cam.SetWindow (0, 0, 512, 768); // Initialize Camera

cam.SetToPosition( Vector3DF(0,0,0) ); // Set camera to look at the origin

cam.SetFromAngles( view );

GameX.SetDrawCam(&cam); // Tell GameX to draw using the camera

// ------------- GAMEX END ---------------

// ------------- GUI BEGIN ---------------

startLogButton = new Button("Start Log", 10, 718, 100, 758);

saveLogButton = new Button("Save Log", 110, 718, 200, 758);

runLogButton = new Button("Run Log", 210, 718, 300, 758);

resetButton = new Button("Reset", 310, 718, 400, 758);

saveLogButton->Disable();

hWnd = GameX.GetWindow();

console = new Console("consoleLog.txt", 10,10,width-10,height-10);

console->Printfn("Motion Capture Modelling System");

console->Printfn("Debug Interface");

accGraph[0] = new Graph("avg_acc1.x", -40, 40, width+0, 0, width+240, 190);

accGraph[1] = new Graph("acc1.y", -40, 40, width+256, 0, width+496, 190);

accGraph[2] = new Graph("acc1.z", -40, 40, width+512, 0, width+752, 190);

velGraph[0] = new Graph("vel1.x", -4, 4, width+0, 192, width+240, 382);

velGraph[1] = new Graph("vel1.y", -0.6, 0.6, width+256, 192, width+496, 382);

velGraph[2] = new Graph("vel1.z", -0.6, 0.6, width+512, 192, width+752, 382);

posGraph[0] = new Graph("pos1.x", -50, 50, width+0, 384, width+240, 574);

posGraph[1] = new Graph("pos1.y", -50, 50, width+256, 384, width+496, 574);

posGraph[2] = new Graph("pos1.z", -50, 50, width+512, 384, width+752, 574);

angGraph[0] = new Graph("psi1", -PI, PI, width+0, 576, width+240, 766);

angGraph[1] = new Graph("phi1", -PI, PI, width+256, 576, width+496, 766);

angGraph[2] = new Graph("theta1", -PI, PI, width+512, 576, width+752, 766);

// ------------- GUI END ---------------

// ------------- MOTION CAPTURE BEGIN ---------------

shoulder = new Point( Vector3DF(0,0,0), 0, NULL, &cam);

elbow = new Elbow( Vector3DF(0,0,-30), 30, shoulder, &cam);

wrist = new Wrist( Vector3DF(0,-60,-30), 30, elbow, &cam);

body = new Body();

// ------------- MOTION CAPTURE END ---------------

// ------------- SERIAL PORT BEGIN ---------------

if (serialPort.Init("COM1") == S_OK)

{

console->Printfn("COM1 Configured Correctly.");

}

else

{

// get error and display it

char lpMsgBuf[100];

serialPort.LastError(lpMsgBuf);

console->Printfn("COM1 Configuration Error: %s", lpMsgBuf );

}

// ------------- SERIAL PORT END ---------------

// ------------- LOG FILE START ---------------

ofn.lStructSize = sizeof(OPENFILENAME);

ofn.hwndOwner = hWnd;

ofn.lpstrFilter = NULL;

ofn.lpstrFile= szFileName;

ofn.nMaxFile = sizeof(szFileName)/ sizeof(*szFileName);

ofn.lpstrFileTitle = NULL;

ofn.nMaxFileTitle = 0;

ofn.lpstrInitialDir = (LPSTR)NULL;

ofn.Flags = OFN_SHOWHELP | OFN_OVERWRITEPROMPT | OFN_CREATEPROMPT;

ofn.lpstrTitle = NULL;

// ------------- LOG FILE END ---------------

wrist->Reset();

}

float wristPsi = 0;

float elbowPhi = 0;

void GameRun (void)

{

DWORD dwWaitResult;

if (readFromLogFile)

{

BytesRead = logFile.ReadLine(ReadBuffer);

if (logFile.IsLogFinished())

{

logFile.Close();

doneReadingLog = true;

readFromLogFile = false;

startLogButton->Enable();

runLogButton->Enable();

}

}

else if( serialPort.Read(ReadBuffer, &BytesRead) == E_FAIL )

{

// get error and display it

char lpMsgBuf[100];

serialPort.LastError(lpMsgBuf);

console->Printfn("Serial Data Read Error: %s", lpMsgBuf );

}

for(int i=0; i<BytesRead; i++)

{

// if buffer is 0x01

if( ReadBuffer[i] == 0x01 )

b_ptr = 0;

else

b[b_ptr++] = ReadBuffer[i];

// if you have a full data packet

if( b_ptr == 8 )

{

b_ptr = 0;

b[7] = ReadBuffer[i];

// unpack the packet into values

accOne.x = (float)b[0] + (float)( (b[6]&0xC0)<<2 );

accOne.y = (float)b[1] + (float)( (b[6]&0x30)<<4 );

accOne.z = (float)b[2] + (float)( (b[6]&0x0C)<<6 );

accTwo.x = (float)b[3] + (float)( (b[6]&0x03)<<8 );

accTwo.y = (float)b[4] + (float)( (b[7]&0xC0)<<2 );

accTwo.z = (float)b[5] + (float)( (b[7]&0x30)<<4 );

// display the data

console->Printfn( "%f, %f, %f, %f, %f, %f", accOne.x, accOne.y, accOne.z, accTwo.x, accTwo.y, accTwo.z );

elbow->Run(accTwo);

wrist->Run(accOne);

body->RotateElbow( elbow->phi, elbow->theta );

body->RotateWrist( wrist->phi, wrist->psi);

//body->RotateElbow( elbowPhi, 0 );

//body->RotateWrist( wristPsi, PI/4);

//elbowPhi += (PI/1920);

//wristPsi += (PI/480);

if (!graphsDisabled)

{

accGraph[0]->AddPoint(wrist->x_avg);//acc.x);

accGraph[1]->AddPoint(wrist->acc.y);

accGraph[2]->AddPoint(wrist->acc.z);

velGraph[0]->AddPoint(wrist->vel.x);

velGraph[1]->AddPoint(wrist->vel.y);

velGraph[2]->AddPoint(wrist->vel.z);

posGraph[0]->AddPoint(wrist->pos.x);

posGraph[1]->AddPoint(wrist->pos.y);

posGraph[2]->AddPoint(wrist->pos.z);

angGraph[0]->AddPoint(wrist->psi);

angGraph[1]->AddPoint(wrist->phi);

angGraph[2]->AddPoint(wrist->theta);

}

}

}

if (logSerialToFile)

{

for (int i = 0; i < BytesRead; i++)

logFile.Printf("%d ", ReadBuffer[i]);

if (BytesRead > 0)

logFile.Printf("\n");

}

if (startLogButton->Run())

{

logFile.OpenForWriting("accLogFile_do_not_touch.txt");

logSerialToFile = true;

doneReadingLog = false;

saveLogButton->Enable();

}

if (saveLogButton->Run())

{

BOOL saveOK = GetSaveFileName(&ofn);

if (saveOK)

{

logFile.Close();

logSerialToFile = false;

doneReadingLog = false;

saveLogButton->Disable();

CopyFile("accLogFile_do_not_touch.txt", ofn.lpstrFile, FALSE);

}

}

if (runLogButton->Run() && !logSerialToFile)

{

BOOL openOK = GetOpenFileName(&ofn);

if (openOK)

{

logFile.OpenForReading(ofn.lpstrFile);

readFromLogFile = true;

doneReadingLog = false;

startLogButton->Disable();

saveLogButton->Disable();

runLogButton->Disable();

}

}

if (resetButton->Run())

{

wrist->Reset();

elbow->Reset();

body->Reset();

}

// move camera view about the origin on right button down

if( GameX.IsMouseDown(MOUSE_RIGHT) ){

view.x += GameX.GetMouseDX();

view.y += GameX.GetMouseDY();

view.z += GameX.GetMouseDZ()/20;

if( view.y < -89.9f ) view.y = -89.9f;

if( view.y > 89.9f ) view.y = 89.9f;

if( view.z < 2.0f ) view.z = 2.0f;

cam.SetToPosition( Vector3DF(0,0,0) );

cam.SetFromAngles( view );

axisView.x = view.x;

axisView.y = view.y;

}

// Toggle Console on '`' key press

if ( GameX.GetKeyPress(KEY_TILDE) )

console->Toggle();

if ( GameX.GetKeyPress(KEY_0) )

graphsDisabled = !graphsDisabled;

if (!graphsDisabled)

{

if ( GameX.GetKeyPress(KEY_1) )

accGraph[0]->Toggle();

if ( GameX.GetKeyPress(KEY_2) )

accGraph[1]->Toggle();

if ( GameX.GetKeyPress(KEY_3) )

accGraph[2]->Toggle();

if ( GameX.GetKeyPress(KEY_Q) )

velGraph[0]->Toggle();

if ( GameX.GetKeyPress(KEY_W) )

velGraph[1]->Toggle();

if ( GameX.GetKeyPress(KEY_E) )

velGraph[2]->Toggle();

if ( GameX.GetKeyPress(KEY_A) )

posGraph[0]->Toggle();

if ( GameX.GetKeyPress(KEY_S) )

posGraph[1]->Toggle();

if ( GameX.GetKeyPress(KEY_D) )

posGraph[2]->Toggle();

if ( GameX.GetKeyPress(KEY_Z) )

angGraph[0]->Toggle();

if ( GameX.GetKeyPress(KEY_X) )

angGraph[1]->Toggle();

if ( GameX.GetKeyPress(KEY_C) )

angGraph[2]->Toggle();

if ( GameX.GetKeyPress(KEY_9) )

{

showAllGraphs = !showAllGraphs;

accGraph[0]->SetShowGraph(showAllGraphs);

accGraph[1]->SetShowGraph(showAllGraphs);

accGraph[2]->SetShowGraph(showAllGraphs);

velGraph[0]->SetShowGraph(showAllGraphs);

velGraph[1]->SetShowGraph(showAllGraphs);

velGraph[2]->SetShowGraph(showAllGraphs);

posGraph[0]->SetShowGraph(showAllGraphs);

posGraph[1]->SetShowGraph(showAllGraphs);

posGraph[2]->SetShowGraph(showAllGraphs);

angGraph[0]->SetShowGraph(showAllGraphs);

angGraph[1]->SetShowGraph(showAllGraphs);

angGraph[2]->SetShowGraph(showAllGraphs);

}

}

}

void drawAxis ()

{

GameX.DrawLine(ColorX(150,150,150), 0, height, width, height);

GameX.DrawLine(ColorX(150,150,150), width, 0, width, height);

cam.SetWindow ( width-100, 0, width, 100 );

cam.SetFromAngles( axisView );

GameX.DrawLine3D (ColorX(255,0,0), Vector3DF(0,0,0), Vector3DF(24,0,0));

GameX.DrawLine3D (ColorX(0,255,0), Vector3DF(0,0,0), Vector3DF(0,24,0));

GameX.DrawLine3D (ColorX(0,0,255), Vector3DF(0,0,0), Vector3DF(0,0,24));

float x, y, z;

cam.Project( Vector3DF(24,0,0), x, y, z);

GameX.DrawText(x-5, y-5, txt_x);

cam.Project( Vector3DF(0,24,0), x, y, z);

GameX.DrawText(x-5, y-5, txt_y);

cam.Project( Vector3DF(0,0,24), x, y, z);

GameX.DrawText(x-5, y-5, txt_z);

cam.SetWindow (0, 0, width, height);

cam.SetFromAngles( view );

sprintf(buffer, "x:%5.3f, y:%5.3f, z:%5.3f", wrist->pos.x, wrist->pos.y, wrist->pos.z);

GameX.DrawText(5,5,buffer);

sprintf(buffer, "vx:%5.3f, vy:%5.3f, vz:%5.3f", wrist->vel.x, wrist->vel.y, wrist->vel.z);

GameX.DrawText(5,20,buffer);

sprintf(buffer, "ax:%5.3f, ay:%5.3f, az:%5.3f", wrist->x_avg, wrist->acc.y, wrist->acc.z);

GameX.DrawText(5,35,buffer);

sprintf(buffer, "phi:%5.3f, psi:%5.3f , theta:%5.3f", wrist->phi, wrist->psi, wrist->theta);

GameX.DrawText(5,50,buffer);

}

void GameDraw (void)

{

GameX.ClearScreen();

// Draw Gray Background

GameX.SetView(0,0,width,height);

GameX.FillScreen (ColorX(100,100,100));

GameX.ResetView();

GameX.Begin3DScene ();

body->Draw();

//wrist->Draw();

//elbow->Draw();

shoulder->Draw();

if (!graphsDisabled)

{

accGraph[0]->Draw();

accGraph[1]->Draw();

accGraph[2]->Draw();

velGraph[0]->Draw();

velGraph[1]->Draw();

velGraph[2]->Draw();

posGraph[0]->Draw();

posGraph[1]->Draw();

posGraph[2]->Draw();

angGraph[0]->Draw();

angGraph[1]->Draw();

angGraph[2]->Draw();

}

startLogButton->Draw();

saveLogButton->Draw();

runLogButton->Draw();

resetButton->Draw();

if (logSerialToFile)

GameX.DrawText(10, 700, "Logging...");

if (readFromLogFile)

GameX.DrawText(10, 700, "Reading from log file...");

if (doneReadingLog)

GameX.DrawText(10, 700, "Done Reading from log file!");

drawAxis ();

GameX.End3DScene ();

console->Draw();

}

A.2.2 Body.hpp

#include "GameX.hpp"

#define PI 3.14159f

#define PI_2 1.570795f

class Body

{

private:

ImageX boxImg;

ImageX redImg;

ImageX blueImg;

ImageX bigRedImg;

ImageX faceImg1;

ImageX faceImg2;