visual_c.cpp

Ph-biped Robot communication code for visual C

#include "stdafx.h"
#include <windows.h>
#include <conio.h>
#include <stdio.h>
#include <io.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <sys\stat.h>



#define SOP_HEAD				100
#define SOP_LEFT_SHOULDER		101
#define SOP_RIGHT_SHOULDER		102
#define SOP_LEFT_ARM			103
#define SOP_RIGHT_ARM			104
#define SOP_LEFT_ARM_TURN		105
#define SOP_RIGHT_ARM_TURN		106
#define SOP_LEFT_ELBOW			107
#define SOP_RIGHT_ELBOW			108
#define SOP_LEFT_WRIST			109
#define SOP_RIGHT_WRIST			110
#define SOP_LEFT_HAND			111
#define SOP_RIGHT_HAND			112
#define SOP_WAIST				113
#define SOP_LEFT_HIP			114
#define SOP_RIGHT_HIP			115
#define SOP_NEWLINE_RSV			116
#define SOP_LEFT_LEG			117
#define SOP_RIGHT_LEG			118
#define SOP_LEFT_LEG_TURN		119
#define SOP_RIGHT_LEG_TURN		120
#define SOP_CR_RSV				121
#define SOP_LEFT_KNEE			122
#define SOP_RIGHT_KNEE			123
#define SOP_LEFT_ANKLE			124
#define SOP_RIGHT_ANKLE			125
#define SOP_LEFT_FOOT			126
#define SOP_RIGHT_FOOT			127
#define SOP_FULL_STATE			129

// USER op from 200 - 240
#define USER_OP_RIGHT_FOOT		200


#define LEFT_HAND_SERVO_POS				0 // for extension
#define HEAD_SERVO_POS				1 
#define LEFT_LEG_TURN_SERVO_POS		2
#define LEFT_HIP_SERVO_POS			3
#define LEFT_LEG_SERVO_POS			4
#define LEFT_KNEE_SERVO_POS			5
#define LEFT_ANKLE_SERVO_POS		6
#define LEFT_FOOT_SERVO_POS			7
#define RIGHT_SHOULDER_SERVO_POS 	8
#define RIGHT_ARM_SERVO_POS			9
#define RIGHT_ELBOW_SERVO_POS		10
#define RIGHT_WRIST_SERVO_POS			11	// for extension
#define RIGHT_HAND_SERVO_POS			12	// for extension
#define WAIST_SERVO_POS				13
#define RIGHT_LEG_TURN_SERVO_POS	14
#define RIGHT_HIP_SERVO_POS			15
#define LEFT_SHOULDER_SERVO_POS		16 
#define LEFT_ARM_SERVO_POS			17 
#define LEFT_ELBOW_SERVO_POS		18 
#define LEFT_WRIST_SERVO_POS			19 // for extension
#define RIGHT_FOOT_SERVO_POS		20
#define RIGHT_ANKLE_SERVO_POS		21
#define RIGHT_KNEE_SERVO_POS		22
#define RIGHT_LEG_SERVO_POS			23
//


#define MAX_PACKET_LEN			36
#define PACKET_SYNC_BYTE1		0xAC
#define PACKET_SYNC_BYTE2		0xCA
#define PACKET_END_BYTE			0xEE
#define SPEED_MASK				0x7
#define MODE_MASK				0x70
#define MODE_SHIFT				4
#define CMD_LENGTH				28
#define SIMPLE_CMD_LENGTH		4
#define OP_RETRY_COUNT			3
//
// definition for motion mode
//
#define CONSTANT_MOVE			0
#define EQUAL_STEP				5
#define WAIT_MODE				7	

HANDLE hCom;
unsigned char outPacket[MAX_PACKET_LEN];
unsigned char inPacket[MAX_PACKET_LEN];
unsigned char outCmdBuf[CMD_LENGTH];
unsigned char inCmdBuf[CMD_LENGTH];


//*********************************************************************
//*
//*  need to assign the correct com port number for your program here
//*
//*********************************************************************
wchar_t pcCommPort[10] = L"COM1";	// for viusal c++ 2008 express edition
//char pcCommPort[10] = "COM1";	// for viusal c++ 


//*********************************************************************
//*
//*  openComPort
//*
//*		- open the com port to communicate with the roboPhilo
//*		- pcCommPort defines the COM port number 
//*		  (some pc may has multiple COM port, check if the one program is the one connected)
//*		- baud defines the baud rate for the COM port openned
//*
//*********************************************************************
HANDLE openComPort(LPCTSTR pcCommPort, int baud) // for viusal c++ 2008 express edition
//HANDLE openComPort(char *pcCommPort, int baud) // for viusal c++ 
{
   DCB dcb;
   HANDLE hCom;
   BOOL fSuccess;
   COMMTIMEOUTS timeouts={0};
   DWORD dwErr;

   hCom = CreateFile( pcCommPort,
                    GENERIC_READ | GENERIC_WRITE,
                    0,    // must be opened with exclusive-access
                    NULL, // no security attributes
                    OPEN_EXISTING, // must use OPEN_EXISTING
                    0,    // not overlapped I/O
                    NULL  // hTemplate must be NULL for comm devices
                    );

   if (hCom == INVALID_HANDLE_VALUE) 
   {
       // Handle the error.
	   dwErr = GetLastError();
	   if (dwErr == 2) {
			printf ("Cannot open COM port, COM port %s does not exist.\n", pcCommPort);
			printf ("Check COM port number.\n");
	   }
	   else
	   if (dwErr == 5) {
			printf ("Cannot open COM port, COM port %s is in used by another program.\n", pcCommPort);
			printf ("Close that program and retry.\n");
	   }
	   else
	   {
			printf ("CreateFile failed with error %d.\n", dwErr);
	   }
       return (NULL);
   }

   fSuccess = GetCommState(hCom, &dcb);

   if (!fSuccess) 
   {
      // Handle the error.
      printf ("GetCommState failed with error %d.\n", GetLastError());
      return (NULL);
   }

   dcb.BaudRate = baud;     // set the baud rate
   dcb.ByteSize = 8;             // data size, xmit, and rcv
   dcb.Parity = NOPARITY;        // no parity bit
   dcb.StopBits = ONESTOPBIT;    // one stop bit

   fSuccess = SetCommState(hCom, &dcb);

   if (!fSuccess) 
   {
      // Handle the error.
      printf ("SetCommState failed with error %d.\n", GetLastError());
      return (NULL);
   }

   timeouts.ReadIntervalTimeout = MAXDWORD;
   timeouts.ReadTotalTimeoutConstant = 5000;	// 5 second timeout
   timeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
   timeouts.WriteTotalTimeoutConstant = 500;
   timeouts.WriteTotalTimeoutMultiplier = 50;

   if (!SetCommTimeouts(hCom, &timeouts)) {
	// Handle the error.
	   printf ("SetCommTimeouts failed with error %d.\n", GetLastError());
	   return (NULL);
	}
   return (hCom);
}

//*********************************************************************
//*
//*  formatPacket
//*
//*		- pack the global outPacket buffer with data, header and checksum to send
//*		- s defines the input data buffer 
//*		- len defines the number of data bytes to pack
//*		- format =  sync byte1, sync byte2, len, [ cmd packet ], chksum, end byte
//*
//*********************************************************************
void formatPacket(unsigned char s[], unsigned char len)
{
unsigned char i;
unsigned char c = 0;

	outPacket[0] = PACKET_SYNC_BYTE1;
	outPacket[1] = PACKET_SYNC_BYTE2;
	outPacket[2] = len;

	for (i=0; i<len; i++) {
		outPacket[i+3] = s[i];
		c = (c + s[i]) & 0xff;	
	}

	outPacket[i+3] = c;
	outPacket[i+4] = PACKET_END_BYTE;
}


//*********************************************************************
//*
//*  setOutCmd
//*
//*		- used for short command (except SOP_FULL_STATE )
//*		- pack the opcode, speed, mode and steps into the data buffer
//*		- outB defines the input data buffer 
//*		- op defines the command op code to send
//*		- count defines the number of data bytes to send
//*		- spd deifnes the speed to execute this command motion
//*		- mode defines the motion mode
//*		- steps defines the number of steps for the motion speed
//*
//*********************************************************************
void setOutCmd(unsigned char * outB, unsigned char op, unsigned char count, unsigned char spd, unsigned char mode, unsigned char step)
{
	unsigned char m;

	m = (((mode << MODE_SHIFT) & MODE_MASK) | (spd  & SPEED_MASK));
	outB[0]=op;
	outB[1]= m;	
	outB[2]=step; 
	outB[3]=count;

	formatPacket(outB, 4);
}


//*********************************************************************
//*
//*  setOutFullCmd
//*
//*		- used for SOP_FULL_STATE command only
//*		- pack the opcode, speed, mode and steps into the data buffer
//*		- outB defines the input data buffer 
//*		- op defines the command op code to send
//*		- count defines the number of data bytes to send
//*		- spd deifnes the speed to execute this command motion
//*		- mode defines the motion mode
//*		- steps defines the number of steps for the motion speed
//*
//*********************************************************************
void setOutFullCmd(unsigned char * outB, unsigned char * pose, unsigned char op, unsigned char spd, unsigned char mode, unsigned char step)
{
	unsigned char * p;

	p = pose;

	outB[0]=op;
	outB[1]=(((mode << MODE_SHIFT) & MODE_MASK) | (spd  & SPEED_MASK));
	outB[2]=step;
//
	outB[LEFT_SHOULDER_SERVO_POS+3] = *p++;
	outB[RIGHT_SHOULDER_SERVO_POS+3] = *p++;
	outB[LEFT_ARM_SERVO_POS+3] = *p++;
	outB[RIGHT_ARM_SERVO_POS+3] = *p++;
	outB[LEFT_ELBOW_SERVO_POS+3] = *p++;
	outB[RIGHT_ELBOW_SERVO_POS+3] = *p++;
	outB[LEFT_WRIST_SERVO_POS+3] = *p++;
	outB[RIGHT_WRIST_SERVO_POS+3] = *p++;
	outB[LEFT_HAND_SERVO_POS+3] = *p++;
	outB[RIGHT_HAND_SERVO_POS+3] = *p++;
	outB[HEAD_SERVO_POS+3] = *p++;
	outB[WAIST_SERVO_POS+3] = *p++;
	outB[LEFT_LEG_TURN_SERVO_POS+3] = *p++;
	outB[RIGHT_LEG_TURN_SERVO_POS+3] = *p++;
	outB[LEFT_HIP_SERVO_POS+3] = *p++;
	outB[RIGHT_HIP_SERVO_POS+3] = *p++;
	outB[LEFT_LEG_SERVO_POS+3] = *p++;
	outB[RIGHT_LEG_SERVO_POS+3] = *p++;
	outB[LEFT_KNEE_SERVO_POS+3] = *p++;
	outB[RIGHT_KNEE_SERVO_POS+3] = *p++;
	outB[LEFT_ANKLE_SERVO_POS+3] = *p++;
	outB[RIGHT_ANKLE_SERVO_POS+3] = *p++;	
	outB[LEFT_FOOT_SERVO_POS+3] = *p++;
	outB[RIGHT_FOOT_SERVO_POS+3] = *p++;
	outB[CMD_LENGTH-1]='\0';	// should add CRC

	formatPacket(outB, CMD_LENGTH);
}

//*********************************************************************
//*
//*  writeCom
//*
//*		- write the data packet to roboPhilo
//*		- h defines the COM port handle
//*		- outB defines the input data buffer 
//*		- nB defines the number of bytes to send
//*
//*********************************************************************
BOOL writeCom( HANDLE h, unsigned char* outB, int nB) 
{
   DWORD nBytesWritten;
   BOOL bResult;
   int j;

	for (j=0;j<nB;j++) {
		bResult = WriteFile(h, &outB[j], 1, &nBytesWritten, NULL) ; 
		if (bResult == NULL) {  
			printf ("Error: writing the string out.\n");
			CloseHandle(h); 
			return (FALSE);
		} 
	}
	return (TRUE);
}


//*********************************************************************
//*
//*  readCom
//*
//*		- read the data packet from roboPhilo
//*		- h defines the COM port handle
//*		- inB defines the input data buffer 
//*		- nB defines the number of bytes to read (don't read more than it returns)
//*
//*********************************************************************
BOOL readCom( HANDLE h, unsigned char* inB,  int nB) 
{
   DWORD nBytesRead;
   BOOL bResult;
   int j;

	for (j=0;j<nB;j++) {
		bResult = ReadFile(h, &inB[j], 1, &nBytesRead, NULL) ; 
		if (bResult == NULL) {  
			printf ("Error: No input.\n");
			CloseHandle(h); 
			return (FALSE);
		} 
	}
	return (TRUE);
}


//*********************************************************************
//*
//*  sendCommand
//*
//*		- send the simple command to RoboPhilo
//*		- op defines the command op code to send
//*		- pos defines the angle to move
//*		- speed deifnes the speed to execute this command motion
//*		- mode defines the motion mode
//*		- steps defines the number of steps for the motion speed
//*
//*********************************************************************
void sendCommand ( unsigned char op,	unsigned char pos, 
			 unsigned char speed, unsigned char mode, unsigned char steps )
{
	unsigned char len;

	setOutCmd(outCmdBuf, op, pos, speed, mode, steps);

	len = outPacket[2] +5;

	if (writeCom(hCom, outPacket, len)) {
		if (readCom(hCom, inCmdBuf, 1)) {	// need to change it to 'a'
			if (inCmdBuf[0] == 'a') {
				return;
			}			
			printf("Error, nack from the robophilo\n");
			return;
		}
		printf("Error, no ack from the robophilo\n");
		return;
	}
	printf("Error, cannot send command to the robophilo\n");
	return;

}


//*********************************************************************
//*
//*  sendPose
//*
//*		- send the full state pose command to RoboPhilo
//*		- speed deifnes the speed to execute this command motion
//*		- mode defines the motion mode
//*		- steps defines the number of steps for the motion speed
//*		- pos defines the 24 servo positions
//*
//*********************************************************************
void sendPose (  unsigned char speed, unsigned char mode, unsigned char steps, unsigned char *pose )
{
	unsigned char len;

	setOutFullCmd(outCmdBuf, pose, SOP_FULL_STATE, speed, mode, steps);

	len = outPacket[2] +5;

	if (writeCom(hCom, outPacket, len)) {
		if (readCom(hCom, inCmdBuf, 1)) {	// need to change it to 'a'
			if (inCmdBuf[0] == 'a') {
				return;
			}
			printf("Error, nack from the robophilo\n");
			return;
		}
		printf("Error, no ack from the robophilo\n");
		return;
	}
	printf("Error, cannot send command to the robophilo\n");
	return;

}


//*********************************************************************
//*
//*  sample main 
//*
//*********************************************************************
// 
//* sample test pose - init position with 2 arms raise to 90 degrees
//*
//*                               {LS  RS LUA RUA  LE  RE  LW  RW LHD RHD HEAD  W LLT RLT  LH  RH  LL  RL  LK  RK  LA  RA  LF  RF}
unsigned char samplePose[24] = {90,90,180,180,90,90,0,0,0,0,90,90,90,90,180,180,146,146,30,30,90,90,90,90};

int _tmain(int argc, _TCHAR* argv[])	// for viusal c++ 2008 express edition
// 
//int main(int argc, char* argv[])	// for visual c++ 6.0
{
	hCom = openComPort(pcCommPort, CBR_600);
	if (hCom > 0) {
		printf ("Serial port %s successfully reconfigured.\n", pcCommPort);
	}
	else {
 		printf ("Error: Serial port %s open failed.\n", pcCommPort);
	}

	// sample to turn the right foot
	sendCommand(SOP_RIGHT_FOOT, 60, 0, EQUAL_STEP, 8 );
	sendCommand(SOP_RIGHT_FOOT, 120, 0, EQUAL_STEP, 8 );
	sendCommand(USER_OP_RIGHT_FOOT, 90, 0, EQUAL_STEP, 8 );

	// sample to send 24 servo pose to raise the arm in the init pose
	sendPose(0, EQUAL_STEP, 12, samplePose);

	// add sample to send user defined operation
	// send over and run runPose, or RunServo.

	return 0;
}