This is the C++ code of the program running inside the Ipaq.

// Implements viewfinder
#include <windows.h>
#include "viewfinder.h"
#include "hpcamapi.h"
// Global variables
HANDLE hVideoThread = NULL;
HWND hWndParent = NULL;
PBYTE pViewfinderData;
volatile BOOL VideoRun = FALSE;
HDC hdcMem = NULL;
int cfactor =440;
int pluser = 0;
int arg;
float mag= 0.15; // THIS RATIO CONVERTS THE A/D VALUE TO A NUMBER (-50 < X < 50);
float magvert=1;
const LXoffset=-10.7;
const LZoffset=2.5;
const CXoffset=8.2;
const CZoffset=0;
const frp=1;
const flp=2;
const brp=3;
const blp=4;
const fry=5;
const fly=6;
const bry=7;
const bly=8;
const toohigh = 11; // (cm) the highest obstacle that can be climbed over
int VFWidth = 240;
int VFHeight = 180;
int Mode = 2;
int Gvalue;
LPCTSTR Hvalue;
int ar;
int al;
float maxR;
float maxL;
float flom;
HANDLE hPort;
float HLangle=9999;
float VLangle=9999;
struct box{
__int8 elevation;
BOOL scaned;
BOOL obstructed;
BOOL toodeep;
BOOL toohigh;
};
box grid[1600];
struct MotorRecord{ //the information required to control the legs is stored in records
LPCVOID question;
int questionlength;
LPCVOID up;
int uplength;
LPCVOID down;
int downlength;
LPCVOID stop;
int stoplength;
int initpos;
int top;
int bottom;
int destination;
int status;
BOOL goingup;
};
MotorRecord leg[9];
int gridnum(int X,int Z)
{
return (((Z+38)*76)+1)+(X+38);
}//***********************************************************************
// Gereral Serial Functions
//***********************************************************************BOOL PortInitialize (LPTSTR lpszPortName)
{
DCB PortDCB;
COMMTIMEOUTS CommTimeouts;
hPort = CreateFile (lpszPortName, // Pointer to the name of the port
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
0,
NULL);
if ( hPort == INVALID_HANDLE_VALUE )
{
//Could not open the port.
PlaySound(TEXT("Type"), NULL, SND_FILENAME );
return FALSE;
}

PortDCB.DCBlength = sizeof (DCB);
//Get the default port setting information.
GetCommState (hPort, &PortDCB);
//Change the DCB structure settings.
PortDCB.BaudRate = 9600; // Current baud
PortDCB.fBinary = TRUE; // Binary mode; no EOF check
PortDCB.fParity = FALSE; // Enable parity checking.
PortDCB.fOutxCtsFlow = FALSE; // No CTS output flow control
PortDCB.fOutxDsrFlow = FALSE; // No DSR output flow control
PortDCB.fDtrControl = DTR_CONTROL_ENABLE;
//DTR flow control type
PortDCB.fDsrSensitivity = FALSE; // DSR sensitivity
PortDCB.fTXContinueOnXoff = TRUE; // XOFF continues Tx
PortDCB.fOutX = FALSE; // No XON/XOFF out flow control
PortDCB.fInX = FALSE; // No XON/XOFF in flow control
PortDCB.fErrorChar = FALSE; // Disable error replacement.
PortDCB.fNull = TRUE; // Disable null stripping.
PortDCB.fRtsControl = RTS_CONTROL_ENABLE;
PortDCB.fAbortOnError = TRUE; // Do not abort reads/writes on
PortDCB.ByteSize = 8; // Number of bits/bytes, 4-8
PortDCB.Parity = NOPARITY; // 0-4=no,odd,even,mark,space
PortDCB.StopBits = ONESTOPBIT; // 0,1,2 = 1, 1.5, 2
if (!SetCommState (hPort, &PortDCB))
{
PlaySound(TEXT("Alarm1"), NULL, SND_FILENAME );
return FALSE;
}
GetCommTimeouts (hPort, &CommTimeouts);
CommTimeouts.ReadIntervalTimeout = MAXDWORD;
CommTimeouts.ReadTotalTimeoutConstant = 10;
CommTimeouts.WriteTotalTimeoutMultiplier = 10;
CommTimeouts.ReadTotalTimeoutMultiplier = 10;
CommTimeouts.WriteTotalTimeoutConstant = 1000;
SetCommTimeouts (hPort, &CommTimeouts);
return TRUE;
}
void PortWrite (LPCVOID thetext,int len)
{
DWORD dwNumBytesWritten;
WriteFile (hPort,
thetext,
len,
&dwNumBytesWritten,
NULL) ;
}
//***********************************************************************
// SV203 Functions
//***********************************************************************
void givesv(LPCVOID thetext, int len,int delay)
{
PortWrite(thetext,len);
PortWrite("\r",1);

Sleep(delay);
}
int asksv(LPCVOID thetext,int len)
{
char gotit[10];
int taken=0;
DWORD dwBytesTransferred;
givesv(thetext,len,5);
Sleep(6);
ReadFile (hPort, gotit,10, &dwBytesTransferred, NULL);
taken=atoi(gotit);
Gvalue=taken;
if (thetext==leg[7].question)
{
taken=100-taken;
}
return taken;
}
void initsv()
{
char gotit[10];
int taken=0;
DWORD dwBytesTransferred;
PortInitialize(TEXT("COM1:"));
while (taken == 0)
{
PortWrite("BD2RE9\r",7);
Sleep(10);
ReadFile (hPort, gotit,10, &dwBytesTransferred, NULL);
taken=atoi(gotit);

if (taken==0)
{
PlaySound(TEXT("Alarm4"), NULL, SND_FILENAME );
}
}
givesv("BD1",3,0);
PlaySound(TEXT("Alarm1"), NULL, SND_FILENAME );
Sleep(1000);
}
//***********************************************************************
// In Depth SV203 Functions
//***********************************************************************
float reduceradian(float radian){
while (radian > 3.14){
radian=radian-6.28;
}
while (radian < -3.14){
radian=radian+6.28;
}
return radian;
}
void setlas(BOOL A)
{
if (A==TRUE){
givesv("BD2PS3",6,3);
}
else
{
givesv("BD2PT3",6,3);
}
}
void movelas(float radian){
int outnum;
char value[3];
radian= reduceradian(-radian);
PortWrite("BD2SV6M",7);
HLangle=radian;
outnum=131+(radian*75);
_itoa(outnum,value, 10);
givesv(value,3,4);
}

//***********************************************************************
// Parallax Vision Functions
//***********************************************************************
COLORREF findcolor(int X, int Y)
{
COLORREF taken;
taken=GetPixel(hdcMem,X,Y);
return taken;
}
void updatesnapshot()
{
int bad = 208;
while (bad == 208)
{
CAMSetWhiteBalance(WB_NONE);
CAMSetAuto(FALSE);
CAMSetRelativeGain(0,0); //100,-100 for laser search
CAMEnableGamma(TRUE);
CAMSetGain(5);
CAMSetShutterSpeed(200);//20
CAMSetMinSpeed(200);
CAMVideoStart();
CAMVideoGetFrame(pViewfinderData);
InvalidateRect(hWndParent,NULL,FALSE);
CAMVideoStop();
bad=GetBValue(findcolor(4,4));
}
}
float findangle(float ax, float ay, float bx, float by)
{
float result=atan2(bx-ax,by-ay);
reduceradian(result);
return result;
}
float sqr(float X){
X=X*X;
return X;
}
float cposTOangle(char axis, int pos)
{
if (axis== 'H')
{
pos=pos-90;
}
else
{
pos=pos-120;
}
float angle=atan2(pos,cfactor);
return angle;
}
int angleTOcpos(char axis, float angle)
{
int center;
if (axis== 'H')
{
center=90;
}
else
{
center=120;
}
int H=center+sin(angle)*cfactor;
return H;
}
BOOL lasercheck( int X, int Y)
{
int R;
COLORREF thecolor;
thecolor=findcolor(X,Y);
R=GetRValue(thecolor);
if (R>40)
{
//PlaySound(TEXT("Notify"), NULL, SND_FILENAME );
return TRUE;
}
else
{
return FALSE;
}
}
int scanrect(int left,int top,int right,int bottom,int detail)
{
int X;
int Y;
BOOL taken=FALSE;
if (left<right && top < bottom && detail>0)
{
Y=top;
while (Y+detail<bottom && taken==FALSE && Y+detail<240)
{
Y=Y+1;

X=left;
while (X+detail<right && taken==FALSE)
{
X=X+1;
taken=lasercheck(Y,X);
}
}
}
return Y;
}
int scansub(int X, int Z)
{
int A ;
// PlaySound(TEXT("MenuPop"), NULL, SND_FILENAME );
// (cm) The physical positions of the camera and laser on the robot
// relative the the siver X
float Langle;
BOOL result,Gresult;
float Cdistance=sqrt(sqr(X+CXoffset)+sqr(Z+CZoffset));
float Ldistance=sqrt(sqr(X+Lxoffset)+sqr(Z+LZoffset));
int margin=400/Cdistance;// divide by (5) is how large of a margin of error from the camera's image is taken
int Cpos=angleTOcpos('H',findangle(X,Z,CXoffset,CZoffset));
if (Cpos+margin< 180 && Cpos-margin > 0)
{
movelas(findangle(LXoffset,LZoffset,X,Z));
setlas(TRUE);
ar=Cpos+margin;
al=Cpos-margin;
Sleep(100);
updatesnapshot();
int Cheight=scanrect(Cpos-margin,0,Cpos+margin,240,1);
if (Cheight <= 1)
{
A=999
}
else


;
if (Cheight >=239)
{
A=-999;
}
else
{
PlaySound(TEXT("HwndSnd"), NULL, SND_FILENAME );
A=sin(cposTOangle('V',Cheight))*Cdistance;
A=6;
}
}
return A;
}
void updategrid(int X,int Y,int color)
{
char rest;
LPRECT block;
//rest=getgrid(X,Y);
HBRUSH BLACK=CreateSolidBrush(PALETTERGB(23*color,23*color,255));
SetRect(block,100+(X*5),200-(Y*5),105+(X*5),205-(Y*5));
OffsetRect(block,-X*5,-Y*5);
PAINTSTRUCT ps;
HDC hdc = BeginPaint(hWndParent,&ps);
FillRect(hdc,block,BLACK);
EndPaint(hWndParent,&ps);
}
//***********************************************************************
// Walking Gait Functions
//***********************************************************************
void initlegs()
{
// LEG[1] = FRONT RIGHT LEG
leg[1].question="BD2AD2";
leg[1].questionlength=6;
leg[1].up="BD1PC1BD2PS6";
leg[1].uplength=12;
leg[1].down = "BD1PS1BD2PS6";
leg[1].downlength = 12;
leg[1].stop = "BD1PC1BD2PC6";
leg[1].stoplength = 12;
leg[1].bottom = 60;
leg[1].top = 150;
// LEG[2] = FRONT LEFT LEG
leg[2].question="BD1AD2";
leg[2].questionlength=6;
leg[2].up="BD2PC5BD1PS2";
leg[2].uplength=12;
leg[2].down = "BD2PS5BD1PS2";
leg[2].downlength = 12;
leg[2].stop = "BD2PC5BD1PC2";
leg[2].stoplength = 12;
leg[2].bottom = 60;
leg[2].top = 150;
// LEG[3] = BACK RIGHT LEG
leg[3].question="BD1AD4 ";
leg[3].questionlength=6;
leg[3].up="BD1SV4M255";
leg[3].uplength=10;
leg[3].down = "BD1SV4M1";
leg[3].downlength = 8;
leg[3].stop = "BD1SV4M0";
leg[3].stoplength = 8;
leg[3].bottom = 89;
leg[3].top = 175;
// LEG[4] = BACK LEFT LEG
leg[4].question="BD2AD1";
leg[4].questionlength=6;
leg[4].up="BD1SV5M1";
leg[4].uplength=8;
leg[4].down = "BD1SV5M255";
leg[4].downlength = 10;
leg[4].stop = "BD1SV5M0";
leg[4].stoplength = 8;
leg[4].bottom = 70;
leg[4].top = 140;
// LEG[5] = FRONT RIGHT YAW
leg[5].question="BD2AD5";
leg[5].questionlength=6;
leg[5].down="BD2SV4M1";
leg[5].downlength=8;
leg[5].up = "BD2SV4M255";
leg[5].uplength = 10;
leg[5].stop = "BD2SV4M0";
leg[5].stoplength = 8;
leg[5].top = 69;
leg[5].bottom = 31;
// LEG[6] = FRONT LEFT PITCH
leg[6].question="BD1AD1";
leg[6].questionlength=6;
leg[6].down="BD2SV2M255";
leg[6].downlength=10;
leg[6].up = "BD2SV2M1";
leg[6].uplength = 8;
leg[6].stop = "BD2SV2M0";
leg[6].stoplength = 8;
leg[6].top = 60;
leg[6].bottom = 25;
// LEG[7] = BACK RIGHT YAW
leg[7].question="BD2AD3";
leg[7].questionlength=6;
leg[7].down="BD2SV7M255";
leg[7].downlength=10;
leg[7].up = "BD2SV7M1";
leg[7].uplength = 8;
leg[7].stop = "BD2SV7M0";
leg[7].stoplength = 8;
leg[7].top = 65;
leg[7].bottom = 30;
// LEG[8] = BACK LEFT P5TCH
leg[8].question="BD1AD5";
leg[8].questionlength=6;
leg[8].up="BD2SV3M1";
leg[8].uplength=8;
leg[8].down = "BD2SV3M255";
leg[8].downlength = 10;
leg[8].stop = "BD2SV3M0";
leg[8].stoplength = 8;
leg[8].top = 220;
leg[8].bottom = 186;
}
int legvalue(int legnum)
{
int out=0;
while (out == 0)
{
out= asksv(leg[legnum].question,leg[legnum].questionlength);
if (out==0)
{
Gvalue=legnum;
}
}
return out;
}
float destinationcoverter(int count,float destination)
{
int factor;
if (count <=4)
{
factor=mag;
}
else
{
factor=magvert;
}
return leg[count].initpos+(destination*factor);
}
void moveleg(int count, int destination)
{
destination=leg[count].initpos+(-destination*mag);
if (abs(destination-legvalue(count)) > 1)
{
leg[count].destination=destination;
if (legvalue(count)<destination)
{
givesv(leg[count].up,leg[count].uplength,40);
leg[count].goingup=TRUE;
}
else
{
givesv(leg[count].down,leg[count].downlength,40);
leg[count].goingup=FALSE;
}
}
}
void pickupmove(int count)
{
givesv(leg[count].up,leg[count].uplength,30);
while(legvalue(count)<leg[count].top-20)
{
}
givesv(leg[count].stop,leg[count].stoplength,30);
givesv(leg[count+4].down,leg[count+4].downlength,30);
while(legvalue(count+4)>leg[count+4].bottom)
{
}
givesv(leg[count+4].stop,leg[count+4].stoplength,30);
givesv(leg[count].down,leg[count].downlength,30);
while(legvalue(count)>(leg[count].bottom+leg[count].top)/2)
{
}
givesv(leg[count].stop,leg[count].stoplength,30);
}
void totalmove(int FR, int FL, int BR, int BL)
{
moveleg(fry,FR);
moveleg(fly,FL);
moveleg(bry,BR);
moveleg(bly,BL);
}
void checklegs()
{
int count=4;
int lval;
while(count < 8)
{
count=count+1;
if (leg[count].destination != 0)
{
lval=legvalue(count);
if (leg[count].goingup==TRUE && lval>leg[count].destination || leg[count].goingup==FALSE && lval<leg[count].destination || abs(lval-leg[count].destination) < 2)
{
givesv(leg[count].stop,leg[count].stoplength,10);
leg[count].destination=0;
}
}
}
}
void stopwait()
{
int count;
BOOL stopit=FALSE;
while(stopit==FALSE)
{
checklegs();
if (leg[1].destination==0 && leg[2].destination==0 && leg[3].destination==0 && leg[4].destination==0 && leg[5].destination==0 && leg[6].destination==0 && leg[7].destination==0 && leg[8].destination==0)
{
stopit=TRUE;
}
}
}
DWORD WINAPI VideoThread(LPVOID lpparam)
{

// THIS IS WHERE SETTINGS ARE TO BE INITIALIZED
maxR=cposTOangle('H',0);
maxL=cposTOangle('H',180);
// SV203 Init
initsv();
givesv("BD0SV1M0",8,50);
givesv("BD0SV2M0",8,50);
givesv("BD0SV3M0",8,50);
givesv("BD0SV4M0",8,50);
givesv("BD0SV5M0",8,50);
givesv("BD0SV6M0",8,50);
givesv("BD0SV7M0",8,50);
givesv("BD0SV8M0",8,50);
givesv("BD0WE1 0",8,50);
givesv("BD0WE2 0",8,50);
givesv("BD0WE3 0",8,50);
givesv("BD0WE4 0",8,50);
givesv("BD0WE5 0",8,50);
givesv("BD0WE6 0",8,50);
givesv("BD0WE7 0",8,50);
givesv("BD0WE8 0",8,50);
// Camera Init
int Counter = 0;
int ppp;
CAMSetWhiteBalance(WB_NONE);
CAMSetAuto(FALSE);
CAMSetRelativeGain(0,0); //100,-100 for laser search
CAMEnableGamma(TRUE);
CAMSetGain(0);
CAMSetShutterSpeed(20);//20
CAMSetMinSpeed(200);
givesv("BD1",3,0);
if (!CAMVideoInit(0,0,VFWidth,VFHeight,Mode,FALSE))
{
MessageBeep(MB_ICONEXCLAMATION); // Invalid parameters!
return 1;
}
CAMVideoStart();
initlegs();
int count=0;
while(count<10)
{
count=count+1;
asksv("BD1AD1",6);
asksv("BD2AD1",6); // clears junk out of SV203 rom
}
count=0;
while(count < 4)
{
count=count+1;
givesv(leg[count].down,leg[count].downlength,40); // if a leg is not down, send it down
leg[count].status=-999;
}
count=0;
while(leg[1].status!= 0 || leg[2].status!=0 || leg[3].status!=0 || leg[4].status!=0) // stop moving a leg once it is down;
{
count=count+1;
if (count>4)
{
count=1;
}
if (legvalue(count)<=((leg[count].bottom+leg[count].top)/2) && leg[count].status!=0)
{
leg[count].status=0;

givesv(leg[count].stop,leg[count].stoplength,40);
}
}
leg[5].initpos=55;
leg[6].initpos=55;
leg[7].initpos=50;
leg[8].initpos=193;
count=4;
while(count<8)
{
count=count+1;
leg[count].top=leg[count].initpos+(mag*50);
leg[count].bottom=leg[count].initpos-(mag*50);
}
count=0;
while(count<8)
{
count=count+1;
leg[count].destination=0;
}
while(true)
{
totalmove(50,-17,-50,17);
stopwait();
pickupmove(3);
totalmove(17,-50,50,-17);
stopwait();
pickupmove(2);
totalmove(-17,50,17,-50);
stopwait();
pickupmove(4);
totalmove(-50,17,-17,50);
stopwait();
pickupmove(1);
}
X=X+1;
flom=atan2(CZoffset-Z,CXoffset-X);
if (flom < -1.57)
{
Z=Z+1;
X=tan(-0.9)*Z;
}
pluser=pluser+1;
arg=scansub(X*4,Z*4);
CAMVideoStop();
CAMClose();


return 0;
}

// ---------------------------------------------------------------------
// Viewfinder Functions
// ---------------------------------------------------------------------

BOOL ViewFinderOpen(HWND hWnd,PBYTE pData)
{
DWORD ThreadID;

if (VideoRun)
return FALSE;

VideoRun = TRUE;
hWndParent = hWnd;
pViewfinderData = pData;

if (!CAMOpen())
{
VideoRun = FALSE;
return FALSE;
}

hVideoThread = CreateThread(NULL,0,VideoThread,NULL,0,&ThreadID);
return TRUE;
}


BOOL ViewFinderClose()
{
VideoRun=FALSE;

if (hVideoThread)
{
HANDLE hWaitEvent[1];
hWaitEvent[0] = hVideoThread;

if (WaitForMultipleObjects(1,hWaitEvent,FALSE,5000)==WAIT_TIMEOUT)
{
MessageBeep(MB_ICONEXCLAMATION);
}

CloseHandle(hVideoThread);
hVideoThread = NULL;
}

return TRUE;
}


BOOL ViewFinderIsRunning()
{
return VideoRun;
}