Mendel - My first Robot (Phoenix like Hexapod)

Hi folks,

I’m just started to working on a pheonix like Hexapod.
The main Goal is an autonomous Robot which can move in my Garden an the first Floor, which means he has to pass an Stair with two Levels.

What I have so far:

  • One SSC to controll the Servos from Lynx.
  • An Max BEC 2 from JETImodel which can draw an max Output from about 20 A (Link).
  • An Arm9-Prototyping-Board from Olimex as Brain (EP9302)
  • A few Hitec 645MG Servos for testing
  • Screws
  • Ball Bearings

Still to buy:

  • LiPo V-MAXX 30C 2S/5500 from Graupner 5500 mAh (Link)
  • Some more 645MG (dreaming of some HSR-5990TG in the future).

Construction:
The Construction and Simulation of the Stresstests I have done with a CAD-Program.
Just a Pic of the current state:

file-server.servemp3.com/download/bot_small.jpg
On the Pic is the BEC on the Top still missing in fact that I don’t have an CAD modell.

I still have to improve the Connection between the lemur parts and between the body parts against twisting.

At the moment I’m just waiting for the CFK so I can start make the CNC-parts.
So far until now.

(Sorry for my english I’m far away from exercise :frowning: )

Greetings to everyone.
Time for a big update:

The construction has been changed a little bit:

I’m using now instead of the ball bearings some plain bearings in fact that they are more resilient against shocks.

file-server.servemp3.com/download/bot2_small.jpg

Just a picture from the milling machine and his owner.
Thanks to him for his patience with me and my ideas :smiley:

file-server.servemp3.com/download/fraese_small.jpg

The tibia part:

file-server.servemp3.com/download/bein_roh.jpg

One of the 5980SG servos with the hinge made of brass:

file-server.servemp3.com/download/servo_mit_gegenlager.jpg

Tibia parts mounted:

file-server.servemp3.com/download/bein_mit_servo.jpg

The coxa parts are milled of two 10 mm glass fiber parts, glued together and sanded.

file-server.servemp3.com/download/huefte_roh_small.jpg
file-server.servemp3.com/download/body_fertig_small.jpg
file-server.servemp3.com/download/body_unten_small.jpg
file-server.servemp3.com/download/body_seite_small.jpg
file-server.servemp3.com/download/huefte_fertig_small.jpg

The total weight is at the moment 1.5 kg. So it seems that my calculations in the begining with 2.2 kg without accu was right.

For the software part I’ve ported Xans Phoenix-Code to C to run it under Linux.

file-server.servemp3.com/download/ik2_small.jpg

At the moment I’ve some problems with powering this beast. For testing I wanted to connect a power supply from an PC, but the voltage break down from 7 V to 4 V.

Has someone an idea? How do you test your Hexapods?

I use an adjustable lab supply, or a set of 6V batteries.

Nice parts! Novel way of making them.

It would be interesting to see your ported C code. But why Linux? You have an Arm 9 board, are you running Linux on it?

Alan KM6VV

I’ve already thought about a lab supply. I must see, what my finance minister says :unamused:

Thanks!

Yes, i’m running debian with kernel 2.6.30.1 on it. I want to use OpenCV in the future. At the moment my code seems to be bug free, but there is a lot room for improvements.

[code]// Project: Hexabot
// Description: Ant, controlled by a PS2 remote
// Software version: V1.0
// Date: 2009-08-17
// Programmer: Daniel Kröhl (aka Chaosmann)
// Basiccode: Phoenix code V1.3 - Jeroen Janssen (aka Xan)
//
// CONTROLS:
// - B11 Turn on/off the bot
// - B10 Switch gaits
// - X/Y-Axis Walk/Strafe
// - Z-Axis Rotate
// - B08 Switch Balance mode on or off
// - B02 & Stick Shift body X/Z/Y an rotate body Y
// - B04 & Stick Rotate body X/Y/Z
// - B05 & Stick Double gait travel length
//
// TODO:
// - Setup serial on start (baudrate etc.)
// - Output to serial in ServoDriver
// - record and replay functions
//
// KNOWN BUGS:
// - None at the moment :wink:

#include
#include <unistd.h>
#include <math.h>
#include <time.h>
#include <stdio.h> // for in-/output
#include <string.h> // strcat
#include <fcntl.h> // für ‘O_RDONLY’ deklaration

#include <termios.h>

#include <linux/joystick.h>

#define JOY_DEV “/dev/input/js0”
#define MODEMDEVICE “/dev/ttyAM1”
#define BAUDRATE B115200

using namespace std;

void gotoxy(int x, int y);
void joystick();
void Gait( int GaitLegNr, int GaitPosX, int GaitPosY, int GaitPosZ, int GaitRotY );
void GaitSeq();
void GaitSelect();
void BalCalcOneLeg (int PosX, int PosZ, int PosY, int BodyOffsetX, int BodyOffsetZ);
void BalanceBody();
void GetSinCos( float AngleDeg );
void GetBoogTan( int BoogTanX, int BoogTanY );
void BodyIK ( int PosX, int PosZ, int PosY, int BodyOffsetX, int BodyOffsetZ, int RotationY );
void LegIK( int IKFeetPosX, int IKFeetPosY, int IKFeetPosZ );
void FreeServos();
void CheckAngles();
void ServoDriver();

//--------------------------------------------------------------------
// [PIN NUMBERS]
const int RRCoxaPin = 0; // Rear Right leg Hip Horizontal
const int RRFemurPin = 1; // Rear Right leg Hip Vertical
const int RRTibiaPin = 2; // Rear Right leg Knee

const int RMCoxaPin = 4; // Middle Right leg Hip Horizontal
const int RMFemurPin = 5; // Middle Right leg Hip Vertical
const int RMTibiaPin = 6; // Middle Right leg Knee

const int RFCoxaPin = 8; // Front Right leg Hip Horizontal
const int RFFemurPin = 9; // Front Right leg Hip Vertical
const int RFTibiaPin = 10; // Front Right leg Knee

const int LRCoxaPin = 16; // Rear Left leg Hip Horizontal
const int LRFemurPin = 17; // Rear Left leg Hip Vertical
const int LRTibiaPin = 18; // Rear Left leg Knee

const int LMCoxaPin = 20; // Middle Left leg Hip Horizontal
const int LMFemurPin = 21; // Middle Left leg Hip Vertical
const int LMTibiaPin = 22; // Middle Left leg Knee

const int LFCoxaPin = 24; // Front Left leg Hip Horizontal
const int LFFemurPin = 25; // Front Left leg Hip Vertical
const int LFTibiaPin = 26; // Front Left leg Knee

//--------------------------------------------------------------------
// [MIN/MAX ANGLES]
const int RRCoxa_MIN = -26; // Mechanical limits of the Right Rear Leg
const int RRCoxa_MAX = 74;
const int RRFemur_MIN = -101;
const int RRFemur_MAX = 95;
const int RRTibia_MIN = -106;
const int RRTibia_MAX = 77;

const int RMCoxa_MIN = -53; // Mechanical limits of the Right Middle Leg
const int RMCoxa_MAX = 53;
const int RMFemur_MIN = -101;
const int RMFemur_MAX = 95;
const int RMTibia_MIN = -106;
const int RMTibia_MAX = 77;

const int RFCoxa_MIN = -58; // Mechanical limits of the Right Front Leg
const int RFCoxa_MAX = 74;
const int RFFemur_MIN = -101;
const int RFFemur_MAX = 95;
const int RFTibia_MIN = -106;
const int RFTibia_MAX = 77;

const int LRCoxa_MIN = -74; // Mechanical limits of the Left Rear Leg
const int LRCoxa_MAX = 26;
const int LRFemur_MIN = -95;
const int LRFemur_MAX = 101;
const int LRTibia_MIN = -77;
const int LRTibia_MAX = 106;

const int LMCoxa_MIN = -53; // Mechanical limits of the Left Middle Leg
const int LMCoxa_MAX = 53;
const int LMFemur_MIN = -95;
const int LMFemur_MAX = 101;
const int LMTibia_MIN = -77;
const int LMTibia_MAX = 106;

const int LFCoxa_MIN = -74; // Mechanical limits of the Left Front Leg
const int LFCoxa_MAX = 58;
const int LFFemur_MIN = -95;
const int LFFemur_MAX = 101;
const int LFTibia_MIN = -77;
const int LFTibia_MAX = 106;

//--------------------------------------------------------------------
// [BODY DIMENSIONS]
const int CoxaLength = 58; // Length of the Coxa [mm]
const int FemurLength = 120; // Length of the Femur [mm]
const int TibiaLength = 130; // Lenght of the Tibia [mm]

const int CoxaAngle = 60; // Default Coxa setup angle

const int RFOffsetX = -65; // Distance X from center of the body to the Right Front coxa
const int RFOffsetZ = -85; // Distance Z from center of the body to the Right Front coxa
const int RMOffsetX = -75; // Distance X from center of the body to the Right Middle coxa
const int RMOffsetZ = 0; // Distance Z from center of the body to the Right Middle coxa
const int RROffsetX = -65; // Distance X from center of the body to the Right Rear coxa
const int RROffsetZ = 85; // Distance Z from center of the body to the Right Rear coxa

const int LFOffsetX = 65; // Distance X from center of the body to the Left Front coxa
const int LFOffsetZ = -85; // Distance Z from center of the body to the Left Front coxa
const int LMOffsetX = 75; // Distance X from center of the body to the Left Middle coxa
const int LMOffsetZ = 0; // Distance Z from center of the body to the Left Middle coxa
const int LROffsetX = 65; // Distance X from center of the body to the Left Rear coxa
const int LROffsetZ = 85; // Distance Z from center of the body to the Left Rear coxa

//--------------------------------------------------------------------
// [REMOTE]
const int TravelDeadZone = 3; // The deadzone for the analog input from the remote

//====================================================================
// [ANGLES]
int RFCoxaAngle; // Actual Angle of the Right Front Leg
int RFFemurAngle;
int RFTibiaAngle;

int RMCoxaAngle; // Actual Angle of the Right Middle Leg
int RMFemurAngle;
int RMTibiaAngle;

int RRCoxaAngle; // Actual Angle of the Right Rear Leg
int RRFemurAngle;
int RRTibiaAngle;

int LFCoxaAngle; // Actual Angle of the Left Front Leg
int LFFemurAngle;
int LFTibiaAngle;

int LMCoxaAngle; // Actual Angle of the Left Middle Leg
int LMFemurAngle;
int LMTibiaAngle;

int LRCoxaAngle; // Actual Angle of the Left Rear Leg
int LRFemurAngle;
int LRTibiaAngle;

//--------------------------------------------------------------------
// [POSITIONS]
int RFPosX; // Actual Position of the Right Front Leg
int RFPosY;
int RFPosZ;

int RMPosX; // Actual Position of the Right Middle Leg
int RMPosY;
int RMPosZ;

int RRPosX; // Actual Position of the Right Rear Leg
int RRPosY;
int RRPosZ;

int LFPosX; // Actual Position of the Left Front Leg
int LFPosY;
int LFPosZ;

int LMPosX; // Actual Position of the Left Middle Leg
int LMPosY;
int LMPosZ;

int LRPosX; // Actual Position of the Left Rear Leg
int LRPosY;
int LRPosZ;

//--------------------------------------------------------------------
// [VARIABLES]

// GetSinCos
float AngleDeg; // Input Angle in degrees
float ABSAngleDeg; // Absolute value of the Angle in Degrees
float AngleRad; // Angle in Radian
float SinA; // Output Sinus of the given Angle
float CosA; // Output Cosinus of the given Angle

// GetBoogTan
int BoogTanX; // Input X
int BoogTanY; // Input Y
float BoogTan; // Output BOOGTAN2(X/Y)

// Body position
int BodyPosX; // Global Input for the position of the body
int BodyPosY;
int BodyPosZ;

// Body Inverse Kinematics
int BodyRotX; // Global Input pitch of the body
int BodyRotY; // Global Input rotation of the body
int BodyRotZ; // Global Input roll of the body
int PosX; // Input position of the feet X
int PosZ; // Input position of the feet Z
int PosY; // Input position of the feet Y
int RotationY; // Input for rotation of a single feet for the gait
int BodyOffsetX; // Input Offset betweeen the body and Coxa X
int BodyOffsetZ; // Input Offset betweeen the body and Coxa Z
float SinB; // Sin buffer for BodyRotX calculations
float CosB; // Cos buffer for BodyRotX calculations
float SinG; // Sin buffer for BodyRotZ calculations
float CosG; // Cos buffer for BodyRotZ calculations
int TotalX; // Total X distance between the center of the body and the feet
int TotalZ; // Total Z distance between the center of the body and the feet
float DistCenterBodyFeet; // Total distance between the center of the body and the feet
float AngleCenterBodyFeetX; // Angle between the center of the body and the feet
int BodyIKPosX; // Output Position X of feet with Rotation
int BodyIKPosY; // Output Position Y of feet with Rotation
int BodyIKPosZ; // Output Position Z of feet with Rotation
short int BodyYShift; // Body Height

// Leg Inverse Kinematics
int IKFeetPosX; // Input position of the Feet X
int IKFeetPosY; // Input position of the Feet Y
int IKFeetPosZ; // Input Position of the Feet Z
int IKFeetPosXZ; // Length between the coxa and feet
float IKSW; // Length between shoulder and wrist
float IKA1; // Angle between SW line and the ground in rad
float IKA2;
bool IKSolution; // Output true if the solution is possible
bool IKSolutionWarning; // Output true if the solution is NEARLY possible
bool IKSolutionError; // Output true if the solution is NOT possible
int IKFemurAngle; // Output Angle of Femur in degrees
int IKTibiaAngle; // Output Angle of Tibia in degrees
int IKCoxaAngle; // Output Angle of Coxa in degrees

//--------------------------------------------------------------------
//[TIMING]
unsigned long lTimerStart; // Start time of the calculation cycles
unsigned long tTimer; // Temp timer

int SSCTime; // Time for servo updates
int PrevSSCTime; // Previous time for the servo updates

unsigned char InputTimeDelay; // Delay that depends on the input to get the “sneaking” effect

//--------------------------------------------------------------------
// [GLOABAL]
bool HexOn; // Switch to turn on Phoenix

//--------------------------------------------------------------------
// [Balance]
bool BalanceMode;
int TravelHeightY;
int TotalTransX;
int TotalTransZ;
int TotalTransY;
int TotalYBal;
int TotalXBal;
int TotalZBal;
int TotalY; // Total Y distance between the center of the body and the feet

// [gait]
unsigned char GaitType; // Gait type
unsigned char NomGaitSpeed; // Nominal speed of the gait

unsigned char LegLiftHeight; // Current Travel height
short int TravelLengthX; // Current Travel length X
short int TravelLengthZ; // Current Travel length Z
short int TravelRotationY; // Current Travel Rotation Y

unsigned char TLDivFactor; // Number of steps that a leg is on the floor while walking
char NrLiftedPos; // Number of positions that a single leg is lifted (1-3)
bool HalfLiftHeigth; // If TRUE the outer positions of the ligted legs will be half height

bool GaitInMotion; // Temp to check if the gait is in motion
unsigned char StepsInGait; // Number of steps in gait
bool LastLeg; // TRUE when the current leg is the last leg of the sequence
unsigned char GaitStep; // Actual Gait step

unsigned char RFGaitLegNr; // Init position of the leg
unsigned char RMGaitLegNr; // Init position of the leg
unsigned char RRGaitLegNr; // Init position of the leg
unsigned char LFGaitLegNr; // Init position of the leg
unsigned char LMGaitLegNr; // Init position of the leg
unsigned char LRGaitLegNr; // Init position of the leg

unsigned char GaitLegNr; // Input Number of the leg

int RFGaitPosX; // Relative position corresponding to the Gait
int RFGaitPosY;
int RFGaitPosZ;
int RFGaitRotY; // Relative rotation corresponding to the Gait

int RMGaitPosX;
int RMGaitPosY;
int RMGaitPosZ;
int RMGaitRotY;

int RRGaitPosX;
int RRGaitPosY;
int RRGaitPosZ;
int RRGaitRotY;

int LFGaitPosX;
int LFGaitPosY;
int LFGaitPosZ;
int LFGaitRotY;

int LMGaitPosX;
int LMGaitPosY;
int LMGaitPosZ;
int LMGaitRotY;

int LRGaitPosX;
int LRGaitPosY;
int LRGaitPosZ;
int LRGaitRotY;

int GaitPosX; // In-/Output Pos X of feet
int GaitPosY; // In-/Output Pos Y of feet
int GaitPosZ; // In-/Output Pos Z of feet
int GaitRotY; // In-/Output Rotation Y of feet

// Serial
int ser_fd;
struct termios oldtio, newtio;

// Joystick
int joy_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x;
char *button=NULL, name_of_joystick[80], *lastbutton=NULL;
struct js_event js;

//====================================================================
// [INIT]
void init()
{
// Serial
if( (ser_fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY | O_NONBLOCK)) == -1 )
{
gotoxy(0,2);
printf( “Couldn’t open serial\n” );
}
tcgetattr(ser_fd, &oldtio);
memset(&newtio, 0, sizeof(newtio));

newtio.c_iflag = IGNBRK | IGNPAR;
newtio.c_oflag = 0;
newtio.c_cflag = BAUDRATE | CREAD | CS8 | CLOCAL;
newtio.c_lflag = 0;

tcflush(ser_fd, TCIFLUSH);
tcsetattr(ser_fd, TCSANOW, &newtio);

memset(&newtio, 0, sizeof(newtio));
tcgetattr(ser_fd, &newtio);

// Joystick
if( ( joy_fd = open( JOY_DEV, O_RDONLY)) == -1 )
{
gotoxy(0,3);
printf( “Couldn’t open joystick\n” );
}

ioctl( joy_fd, JSIOCGAXES, &num_of_axis );
ioctl( joy_fd, JSIOCGBUTTONS, &num_of_buttons );
ioctl( joy_fd, JSIOCGNAME(80), &name_of_joystick );

axis = (int *) calloc( num_of_axis, sizeof( int ) );
button = (char *) calloc( num_of_buttons, sizeof( char ) );
lastbutton = (char *) calloc( num_of_buttons, sizeof( char ) );

gotoxy(0,3);
printf(“Joystick detected: %s\t%d axis\t%d buttons”
, name_of_joystick
, num_of_axis
, num_of_buttons );

fcntl( joy_fd, F_SETFL, O_NDELAY ); /* use non-blocking mode */

// Feet Positions
RFPosX = 53;				// Start positions of the Right Front leg
RFPosY = 25;
RFPosZ = -91;

RMPosX = 105;				// Start positions of the Right Middle leg
RMPosY = 25;
RMPosZ = 0;

RRPosX = 53;				// Start positions of the Right Rear leg
RRPosY = 25;
RRPosZ = 91;

LFPosX = 53;				// Start positions of the Left Front leg
LFPosY = 25;
LFPosZ = -91;

LMPosX = 105;				// Start positions of the Left Middle leg
LMPosY = 25;
LMPosZ = 0;

LRPosX = 53;				// Start positions of the Left Rear leg
LRPosY = 25;
LRPosZ = 91;

// Body Positions
BodyPosX = 0;
BodyPosY = 0;
BodyPosZ = 0;

// Body Rotations
BodyRotX = 0;
BodyRotY = 0;
BodyRotZ = 0;

// Gait
GaitType = 0;
BalanceMode = 0;
LegLiftHeight = 50;
GaitStep = 1;
GaitSelect();

// Body height
BodyYShift = 0;

// SSC
SSCTime = 150;
HexOn = false;
FreeServos();

}

//====================================================================
// [MAIN]
int main()
{
init();

while (true)
{
	// Start time
	lTimerStart = clock()/1000;
	tTimer = lTimerStart;

	// Reset IKsolution indicators
	IKSolution = false;
	IKSolutionWarning = false;
	IKSolutionError = false;

	// Gait
	GaitSeq();

	// Balance calculations
	TotalTransX = 0;			// reset values used for calculation of balance
	TotalTransZ = 0;
	TotalTransY = 0;
	TotalXBal = 0;
	TotalYBal = 0;
	TotalZBal = 0;
	if (BalanceMode>0)
	{
		BalCalcOneLeg( -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ );
		BalCalcOneLeg( -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ );
		BalCalcOneLeg( -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ );
		BalCalcOneLeg( LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ );
		BalCalcOneLeg( LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ );
		BalCalcOneLeg( LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ );
		BalanceBody();
	}

	// Reset IKsolution indicators
	IKSolution = false;
	IKSolutionWarning = false;
	IKSolutionError = false;

	// Right Front leg
	BodyIK( -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+BodyYShift+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY);
	LegIK( RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyYShift-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ);
	RFCoxaAngle  = IKCoxaAngle + CoxaAngle;		//Angle for the basic setup for the front leg
	RFFemurAngle = IKFemurAngle;
	RFTibiaAngle = IKTibiaAngle;

	// Right Middle leg
	BodyIK( -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+BodyYShift+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY);
	LegIK( RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY+BodyYShift-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ);
	RMCoxaAngle  = IKCoxaAngle;
	RMFemurAngle = IKFemurAngle;
	RMTibiaAngle = IKTibiaAngle;

	// Right Rear leg
	BodyIK( -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+BodyYShift+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY );
	LegIK( RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyYShift-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ );
	RRCoxaAngle  = IKCoxaAngle - CoxaAngle;		// Angle for the basic setup for the front leg
	RRFemurAngle = IKFemurAngle;
	RRTibiaAngle = IKTibiaAngle;

	// Left Front leg
	BodyIK( LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+BodyYShift+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY );
	LegIK( LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY+BodyYShift-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ );
	LFCoxaAngle  = IKCoxaAngle + CoxaAngle;		// Angle for the basic setup for the front leg
	LFFemurAngle = IKFemurAngle;
	LFTibiaAngle = IKTibiaAngle;

	// Left Middle leg
	BodyIK( LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+BodyYShift+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY );
	LegIK( LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY+BodyYShift-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ );
	LMCoxaAngle  = IKCoxaAngle;
	LMFemurAngle = IKFemurAngle;
	LMTibiaAngle = IKTibiaAngle;

	// Left Rear leg
	BodyIK( LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY );
	LegIK( LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ );
	LRCoxaAngle  = IKCoxaAngle - CoxaAngle;		// Angle for the basic setup for the front leg
	LRFemurAngle = IKFemurAngle;
	LRTibiaAngle = IKTibiaAngle;

	CheckAngles();

	// Read input
	joystick();
	
  gotoxy(0,16); cout << "GaitStep: " << (int)GaitStep << "  ";
  gotoxy(40,15); cout << "TravelX: " << (int)TravelLengthX << "  ";
  gotoxy(60,15); cout << "TravelZ: " << (int)TravelLengthZ << "  ";
  gotoxy(20,14); cout << "tTimer: " << tTimer;
  gotoxy(40,14); cout << "Start: " << lTimerStart;

	if( HexOn ) {
		// Wait for previous commands to be completed while walking
		if ( (abs(TravelLengthX)>TravelDeadZone | abs(TravelLengthZ)>TravelDeadZone | abs(TravelRotationY*2)>TravelDeadZone) ) {
  	if ( BalanceMode == 0)
  			SSCTime = NomGaitSpeed + (InputTimeDelay*2);
  		else
  			SSCTime = NomGaitSpeed + (InputTimeDelay*2) + 100;
			while(lTimerStart + PrevSSCTime > tTimer)
				tTimer = clock()/1000;
		}
		else {
			SSCTime = 200; 				//NomGaitSpeed
		}
		ServoDriver();
	}
}

close( joy_fd ); /* too bad we never get here */
close( ser_fd );
}

void gotoxy(int x, int y)
{
printf("%c%d;%df",0x1B,y,x);
}

//--------------------------------------------------------------------
// [Joystick Input]
void joystick()
{
int x;
for (x = 0; x < 18; x++)
{
/* read the joystick state */
read(joy_fd, &js, sizeof(struct js_event));

/* see what to do with the event */
  switch (js.type & ~JS_EVENT_INIT)
{
	case JS_EVENT_AXIS:
  	axis    js.number ] = js.value;
      break;
  case JS_EVENT_BUTTON:
	  button  js.number ] = js.value;
  	break;
  }
}

/* work with the data */
if (lastbutton[11] == 1 && button[11] == 0) // Start Button test
{
if (HexOn)
{
// Turn off
BodyPosX = 0;
BodyPosY = 0;
BodyPosZ = 0;
BodyRotX = 0;
BodyRotY = 0;
BodyRotZ = 0;
TravelLengthX = 0;
TravelLengthZ = 0;
TravelRotationY = 0;

	  SSCTime = 600;
  	ServoDriver();
  	HexOn = false;
		FreeServos();
		gotoxy(0,1); printf( "Power: OFF" );
	}
	else
	{
  	// Turn on
	  SSCTime = 200;
	  HexOn = true;
		gotoxy(0,1); printf( "Power: ON " );
	}
}

if (HexOn)
{
	if ( lastbutton[10] == 1 && button[10] == 0 )			// Gait-Select Button test
	{
  	if ( TravelLengthX==0 & TravelLengthZ==0 & TravelRotationY==0 )
  	{
	  	// Switch to next Gait type
		if ( GaitType < 7 )
			  GaitType = GaitType+1;
  	else
			  GaitType = 0;
  		GaitSelect();
  	}
}

	if ( lastbutton[8] == 1 && button[8] == 0 )				// BalanceMode Button test
	{
  	if ( BalanceMode == 0 )
  	{
			BalanceMode = 1;
			gotoxy(20,1); printf( "Balance mode: ON " );
		}
  	else
  	{
			BalanceMode = 0;
			gotoxy(20,1); printf( "Balance mode: OFF" );
		}
	}	

	// Throttle-Axis as Body height
 	BodyYShift = (int)(-axis[2]/256/2);

	if (button[2] == 1)									//L1 Button test
	{
  	BodyPosX = (int)(axis[0]/256/2);
	  BodyPosZ = -(int)(axis[1]/256/3);
	  BodyRotY = (int)(axis[3]/256/6);
	}
	else if (button[4] == 1)						// L2 Button test
	{
	  BodyRotX = (axis[1]/256)/8;
	  BodyRotY = (axis[3]/256)/6;
	  BodyRotZ = (axis[0]/256)/8;
	}
	else																// Walk
	{
  	if (button[5] == 1)								// R2 Button test
  	{
			TravelLengthX = -(axis[0]/256);
			TravelLengthZ = (axis[1]/256);
		}
  	else
  	{
			TravelLengthX = -(axis[0]/256)/2;
			TravelLengthZ = (axis[1]/256)/2;
		}		
  	TravelRotationY = -(axis[3]/256)/4;
	}

	//Calculate walking time delay
	InputTimeDelay = 128 - max(max(abs((axis[0]/256)), abs((axis[1]/256))), abs((axis[3]/256)));
}

for (int nC = 0; nC < num_of_buttons; nC++)
	lastbutton[nC] = button[nC];

/* print the results */
gotoxy(0,4);
printf( "X: %6d Y: %6d ", axis[0], axis[1] );

if( num_of_axis > 3 )
printf("Z: %6d ", axis[3] );

if( num_of_axis > 2 )
printf("T: %6d ", axis[2] );

if( num_of_axis > 4 )
printf("CX: %6d ", axis[4] );

if( num_of_axis > 5 )
printf("CY: %6d ", axis[5] );

gotoxy(0,5);
for( x=0 ; x<num_of_buttons ; ++x )
printf("B%d: %d ", x, button[x] );

fflush(stdout);
}

//--------------------------------------------------------------------
// [GAIT Select]
void GaitSelect()
{
// Gait selector
if ( GaitType == 0 ) { // Ripple Gait 6 steps
LRGaitLegNr = 1;
RFGaitLegNr = 2;
LMGaitLegNr = 3;
RRGaitLegNr = 4;
LFGaitLegNr = 5;
RMGaitLegNr = 6;

	NrLiftedPos = 1;
	TLDivFactor = 4;
	StepsInGait = 6;
	NomGaitSpeed = 150;
	gotoxy(40,1); printf("Gait: Ripple Gait 6 steps ");
}

if ( GaitType == 1 ) {		// Ripple Gait 12 steps
	LRGaitLegNr = 1;
	RFGaitLegNr = 3;
	LMGaitLegNr = 5;
	RRGaitLegNr = 7;
	LFGaitLegNr = 9;
	RMGaitLegNr = 11;

	NrLiftedPos = 3;
	HalfLiftHeigth = true;
	TLDivFactor = 8;
	StepsInGait = 12;
	NomGaitSpeed = 100;
	gotoxy(40,1); printf("Gait: Ripple Gait 12 steps");
}

if ( GaitType == 2 ) {		// Quadripple 9 steps
	LRGaitLegNr = 1;
	RFGaitLegNr = 2;
	LMGaitLegNr = 4;
	RRGaitLegNr = 5;
	LFGaitLegNr = 7;
	RMGaitLegNr = 8;

	NrLiftedPos = 2;
	HalfLiftHeigth = false;
	TLDivFactor = 6;
	StepsInGait = 9;
	NomGaitSpeed = 150;
	gotoxy(40,1); printf("Gait: Quadripple 9 steps  ");
}

if ( GaitType == 3 ) {		// Tripod 4 steps
	LRGaitLegNr = 3;
	RFGaitLegNr = 1;
	LMGaitLegNr = 1;
	RRGaitLegNr = 1;
	LFGaitLegNr = 3;
	RMGaitLegNr = 3;

	NrLiftedPos = 1;
	TLDivFactor = 2;
	StepsInGait = 4;
	NomGaitSpeed = 150;
	gotoxy(40,1); printf("Gait: Tripod 4 steps      ");
}

if ( GaitType == 4 ) {		// Tripod 6 steps
	LRGaitLegNr = 4;
	RFGaitLegNr = 1;
	LMGaitLegNr = 1;
	RRGaitLegNr = 1;
	LFGaitLegNr = 4;
	RMGaitLegNr = 4;

	NrLiftedPos = 2;
	HalfLiftHeigth = false;
	TLDivFactor = 4;
	StepsInGait = 6;
	NomGaitSpeed = 150;
	gotoxy(40,1); printf("Gait: Tripod 6 steps      ");
}

if ( GaitType == 5 ) {		// Tripod 8 steps
	LRGaitLegNr = 5;
	RFGaitLegNr = 1;
	LMGaitLegNr = 1;
	RRGaitLegNr = 1;
	LFGaitLegNr = 5;
	RMGaitLegNr = 5;

	NrLiftedPos = 3;
	HalfLiftHeigth = true;
	TLDivFactor = 4;
	StepsInGait = 8;
	NomGaitSpeed = 110;
	gotoxy(40,1); printf("Gait: Tripod 8 steps      ");
}

if ( GaitType == 6 ) {		// Wave 12 steps
	LRGaitLegNr = 7;
	RFGaitLegNr = 1;
	LMGaitLegNr = 9;
	RRGaitLegNr = 5;
	LFGaitLegNr = 11;
	RMGaitLegNr = 3;

	NrLiftedPos = 1;
	HalfLiftHeigth = false;
	TLDivFactor = 10;
	StepsInGait = 12;
	NomGaitSpeed = 120;
	gotoxy(40,1); printf("Gait: Wave 12 steps       ");
}

if ( GaitType == 7 ) {		// Wave 18 steps
	LRGaitLegNr = 10;
	RFGaitLegNr = 1;
	LMGaitLegNr = 13;
	RRGaitLegNr = 7;
	LFGaitLegNr = 16;
	RMGaitLegNr = 4;

	NrLiftedPos = 2;
	HalfLiftHeigth = false;
	TLDivFactor = 16;
	StepsInGait = 18;
	NomGaitSpeed = 100;
	gotoxy(40,1); printf("Gait: Wave 18 steps       ");
}

}

//--------------------------------------------------------------------
// [GAIT Sequence]
void GaitSeq()
{
//Calculate Gait sequence
LastLeg = false;
Gait( LRGaitLegNr, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY );
LRGaitPosX = GaitPosX;
LRGaitPosY = GaitPosY;
LRGaitPosZ = GaitPosZ;
LRGaitRotY = GaitRotY;
gotoxy( 0,20); cout << "LR X: " << (int)GaitPosX << " ";
gotoxy(10,20); cout << " Y: " << (int)GaitPosY << " ";
gotoxy(18,20); cout << " Z: " << (int)GaitPosZ << " ";
gotoxy(26,20); cout << " RY: " << (int)GaitRotY << " ";

Gait( RFGaitLegNr, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY );
RFGaitPosX = GaitPosX;
RFGaitPosY = GaitPosY;
RFGaitPosZ = GaitPosZ;
RFGaitRotY = GaitRotY;
gotoxy(39,18); cout << "RF X: " << (int)GaitPosX << " ";
gotoxy(49,18); cout << " Y: " << (int)GaitPosY << " ";
gotoxy(57,18); cout << " Z: " << (int)GaitPosZ << " ";
gotoxy(65,18); cout << " RY: " << (int)GaitRotY << " ";

Gait( LMGaitLegNr, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY );
LMGaitPosX = GaitPosX;
LMGaitPosY = GaitPosY;
LMGaitPosZ = GaitPosZ;
LMGaitRotY = GaitRotY;
gotoxy( 0,19); cout << "LM X: " << (int)GaitPosX << " ";
gotoxy(10,19); cout << " Y: " << (int)GaitPosY << " ";
gotoxy(18,19); cout << " Z: " << (int)GaitPosZ << " ";
gotoxy(26,19); cout << " RY: " << (int)GaitRotY << " ";

Gait( RRGaitLegNr, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY );
RRGaitPosX = GaitPosX;
RRGaitPosY = GaitPosY;
RRGaitPosZ = GaitPosZ;
RRGaitRotY = GaitRotY;
gotoxy(39,20); cout << "RR X: " << (int)GaitPosX << "   ";
gotoxy(49,20); cout << " Y: " << (int)GaitPosY << "   ";
gotoxy(57,20); cout << " Z: " << (int)GaitPosZ << "   ";
gotoxy(65,20); cout << " RY: " << (int)GaitRotY << "   ";

Gait( LFGaitLegNr, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY );
LFGaitPosX = GaitPosX;
LFGaitPosY = GaitPosY;
LFGaitPosZ = GaitPosZ;
LFGaitRotY = GaitRotY;
gotoxy( 0,18); cout << "LF X: " << (int)GaitPosX << "   ";
gotoxy(10,18); cout << " Y: " << (int)GaitPosY << "   ";
gotoxy(18,18); cout << " Z: " << (int)GaitPosZ << "   ";
gotoxy(26,18); cout << " RY: " << (int)GaitRotY << "   ";

LastLeg = true;
Gait( RMGaitLegNr, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY );
RMGaitPosX = GaitPosX;
RMGaitPosY = GaitPosY;
RMGaitPosZ = GaitPosZ;
RMGaitRotY = GaitRotY;
gotoxy(39,19); cout << "RM X: " << (int)GaitPosX << "   ";
gotoxy(49,19); cout << " Y: " << (int)GaitPosY << "   ";
gotoxy(57,19); cout << " Z: " << (int)GaitPosZ << "   ";
gotoxy(65,19); cout << " RY: " << (int)GaitRotY << "   ";

}

//--------------------------------------------------------------------
// [GAIT]
void Gait( int iGaitLegNr, int iGaitPosX, int iGaitPosY, int iGaitPosZ, int iGaitRotY )
{
// Check IF the Gait is in motion
GaitInMotion = ((abs(TravelLengthX)>TravelDeadZone) || (abs(TravelLengthZ)>TravelDeadZone) || (abs(TravelRotationY)>TravelDeadZone) );
gotoxy(20,15); cout << "GaitMotion: " << (int)GaitInMotion;

// Leg middle up position
// Gait in motion Gait NOT in motion, return to home position
if ( (GaitInMotion && (NrLiftedPos==1 || NrLiftedPos==3) && GaitStep==iGaitLegNr) || (GaitInMotion==false && GaitStep==iGaitLegNr && ((abs(iGaitPosX)>2) || (abs(iGaitPosZ)>2) || (abs(iGaitRotY)>2))) ) // Up
{
GaitPosX = 0;
GaitPosY = -LegLiftHeight;
GaitPosZ = 0;
GaitRotY = 0;
}
else
{
// Optional Half heigth Rear
if ( ((NrLiftedPos==2 && GaitStep==iGaitLegNr) || (NrLiftedPos==3 & (GaitStep==iGaitLegNr-1 || GaitStep==iGaitLegNr+(StepsInGait-1)))) && GaitInMotion )
{
GaitPosX = -TravelLengthX/2;
GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1);
GaitPosZ = -TravelLengthZ/2;
GaitRotY = -TravelRotationY/2;
}
else
{
// Optional half heigth front
if ( (NrLiftedPos>=2) && (GaitStep==iGaitLegNr+1 || GaitStep==iGaitLegNr-(StepsInGait-1)) && GaitInMotion )
{
GaitPosX = TravelLengthX / 2;
GaitPosY = -LegLiftHeight / ( HalfLiftHeigth + 1 );
GaitPosZ = TravelLengthZ / 2;
GaitRotY = TravelRotationY / 2;
}
else
{
// Leg front down position
if ( (GaitStep==iGaitLegNr+NrLiftedPos || GaitStep==iGaitLegNr-(StepsInGait-NrLiftedPos)) && iGaitPosY<0 )
{
GaitPosX = TravelLengthX / 2;
GaitPosY = 0;
GaitPosZ = TravelLengthZ / 2;
GaitRotY = TravelRotationY / 2;
}
// Move body forward
else
{
GaitPosX = iGaitPosX - (TravelLengthX/TLDivFactor);
GaitPosY = 0;
GaitPosZ = iGaitPosZ - (TravelLengthZ/TLDivFactor);
GaitRotY = iGaitRotY - (TravelRotationY/TLDivFactor);
}
}
}
}
gotoxy(20,16); cout << "Gait X: " << (int)GaitPosX << " ";
gotoxy(32,16); cout << " Y: " << (int)GaitPosY << " ";
gotoxy(40,16); cout << " Z: " << (int)GaitPosZ << " ";
gotoxy(48,16); cout << " RY: " << (int)GaitRotY << " ";
// Advance to the next step
if ( LastLeg ) //The last leg in this step
{
GaitStep = GaitStep + 1;
if ( GaitStep>StepsInGait )
{
GaitStep = 1;
}
}
}

//--------------------------------------------------------------------
// [BalCalcOneLeg]
void BalCalcOneLeg (int PosX, int PosZ, int PosY, int BodyOffsetX, int BodyOffsetZ)
{
//Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ + PosZ;
TotalX = BodyOffsetX + PosX;
TotalY = 150 + PosY; // using the value 150 to lower the centerpoint of rotation 'BodyPosY +
TotalTransY = TotalTransY + PosY;
TotalTransZ = TotalTransZ + TotalZ;
TotalTransX = TotalTransX + TotalX;
GetBoogTan( TotalX, TotalZ );
TotalYBal = TotalYBal + (int)((BoogTan180.0) / 3.141592);
GetBoogTan( TotalX, TotalY );
TotalZBal = TotalZBal + (int)((BoogTan
180.0) / 3.141592);
GetBoogTan( TotalZ, TotalY );
TotalXBal = TotalXBal + (int)((BoogTan*180.0) / 3.141592);
////serout S_OUT, i9600, “BalOneLeg PosX=”, sdec PosX," PosZ=", sdec PosZ," TotalXTransZ=", sdec TotalTransZ, 13]
}

//--------------------------------------------------------------------
// [BalanceBody]
void BalanceBody()
{
TotalTransZ = TotalTransZ / 6;
TotalTransX = TotalTransX / 6;
TotalTransY = TotalTransY / 6;
if ( TotalYBal < -180 ) // Tangens fix caused by +/- 180 deg
TotalYBal = TotalYBal + 360;
if ( TotalZBal < -180 ) // Tangens fix caused by +/- 180 deg
TotalZBal = TotalZBal + 360;
if ( TotalXBal < -180 ) // Tangens fix caused by +/- 180 deg
TotalXBal = TotalXBal + 360;

// Balance rotation
TotalYBal = TotalYBal / 6;
TotalXBal = TotalXBal / 6;
TotalZBal = -TotalZBal / 6;

// Balance translation
LFGaitPosZ = LFGaitPosZ - TotalTransZ;
LMGaitPosZ = LMGaitPosZ - TotalTransZ;
LRGaitPosZ = LRGaitPosZ - TotalTransZ;
RFGaitPosZ = RFGaitPosZ - TotalTransZ;
RMGaitPosZ = RMGaitPosZ - TotalTransZ;
RRGaitPosZ = RRGaitPosZ - TotalTransZ;

LFGaitPosX = LFGaitPosX - TotalTransX;
LMGaitPosX = LMGaitPosX - TotalTransX;
LRGaitPosX = LRGaitPosX - TotalTransX;
RFGaitPosX = RFGaitPosX - TotalTransX;
RMGaitPosX = RMGaitPosX - TotalTransX;
RRGaitPosX = RRGaitPosX - TotalTransX;

LFGaitPosY = LFGaitPosY - TotalTransY;
LMGaitPosY = LMGaitPosY - TotalTransY;
LRGaitPosY = LRGaitPosY - TotalTransY;
RFGaitPosY = RFGaitPosY - TotalTransY;
RMGaitPosY = RMGaitPosY - TotalTransY;
RRGaitPosY = RRGaitPosY - TotalTransY;

}

//--------------------------------------------------------------------
// [GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles
// AngleDeg - Input Angle in degrees
// SinA - Output Sinus of AngleDeg
// CosA - Output Cosinus of AngleDeg
void GetSinCos( float AngleDeg )
{
// Get the absolute value of AngleDeg
if ( AngleDeg < 0.0 )
ABSAngleDeg = AngleDeg *-1.0;
else
ABSAngleDeg = AngleDeg;

// Shift rotation to a full circle of 360 deg -> AngleDeg // 360
if ( AngleDeg < 0.0 )		// Negative values
	AngleDeg = 360.0-(ABSAngleDeg-(float)(360*((int)(ABSAngleDeg/360.0))));
else						// Positive values
	AngleDeg = ABSAngleDeg-(float)(360*((int)(ABSAngleDeg/360.0)));

if ( AngleDeg < 180.0 ) {	// Angle between 0 and 180
	// Subtract 90 to shift range
	AngleDeg = AngleDeg -90.0;
	// Convert degree to radials
	AngleRad = (AngleDeg*3.141592)/180.0;

	SinA = cos(AngleRad);	// Sin o to 180 deg = cos(Angle Rad - 90deg)
	CosA = -sin(AngleRad);	// Cos 0 to 180 deg = -sin(Angle Rad - 90deg)
}
else {						// Angle between 180 and 360
	// Subtract 270 to shift range
	AngleDeg = AngleDeg -270.0;
	// Convert degree to radials
	AngleRad = (AngleDeg*3.141592)/180.0;

	SinA = -cos(AngleRad);	// Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
	CosA = sin(AngleRad);	// Cos 180 to 360 deg = sin(Angle Rad - 270deg)
}

}

//--------------------------------------------------------------------
// [BOOGTAN2] Gets the Inverse Tangus from X/Y with the where Y can be zero or negative
// BoogTanX - Input X
// BoogTanY - Input Y
// BoogTan - Output BOOGTAN2(X/Y)
void GetBoogTan( int BoogTanX, int BoogTanY )
{
if ( BoogTanX == 0 ) { // X=0 -> 0 or PI
if (BoogTanY >= 0) {
BoogTan = 0.0;
}
else {
BoogTan = 3.141592;
}
}
if ( BoogTanY == 0 ) { // Y=0 -> +/- Pi/2
if ( BoogTanX > 0 ) {
BoogTan = 3.141592 / 2.0;
}
else {
BoogTan = -3.141592 / 2.0;
}
}
else {
if ( BoogTanY > 0 ) { // BOOGTAN(X/Y)
BoogTan = atan((float)(BoogTanX) / (float)(BoogTanY));
}
else {
if ( BoogTanX > 0 ) { //BOOGTAN(X/Y) + PI
BoogTan = atan((float)(BoogTanX) / (float)(BoogTanY)) + 3.141592;
}
else { // BOOGTAN(X/Y) - PI
BoogTan = atan((float)(BoogTanX) / (float)(BoogTanY)) - 3.141592;
}
}
}
}

//--------------------------------------------------------------------
// [BODY INVERSE KINEMATICS]
// BodyRotX - Global Input pitch of the body
// BodyRotY - Global Input rotation of the body
// BodyRotZ - Global Input roll of the body
// RotationY - Input Rotation for the gait
// PosX - Input position of the feet X
// PosZ - Input position of the feet Z
// BodyOffsetX - Input Offset betweeen the body and Coxa X
// BodyOffsetZ - Input Offset betweeen the body and Coxa Z
// SinB - Sin buffer for BodyRotX
// CosB - Cos buffer for BodyRotX
// SinG - Sin buffer for BodyRotZ
// CosG - Cos buffer for BodyRotZ
// BodyIKPosX - Output Position X of feet with Rotation
// BodyIKPosY - Output Position Y of feet with Rotation
// BodyIKPosZ - Output Position Z of feet with Rotation
void BodyIK ( int PosX, int PosZ, int PosY, int BodyOffsetX, int BodyOffsetZ, int RotationY )
{
//Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ;
TotalX = BodyOffsetX+PosX;
//PosY are equal to a “TotalY”

//Successive global rotation matrix:
//Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
//Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...

//First calculate sinus and cosinus for each rotation:
GetSinCos((float)(BodyRotX+TotalXBal));
SinG = SinA;
CosG = CosA;
GetSinCos((float)(BodyRotZ+TotalZBal));
SinB = SinA;
CosB = CosA;
GetSinCos((float)(BodyRotY+RotationY+TotalYBal));

//Calcualtion of rotation matrix:
BodyIKPosX = TotalX-(int)((float)(TotalX)*CosA*CosB - (float)(TotalZ)*CosB*SinA + (float)(PosY)*SinB);
BodyIKPosZ = TotalZ-(int)((float)(TotalX)*CosG*SinA + (float)(TotalX)*CosA*SinB*SinG +(float)(TotalZ)*CosA*CosG-(float)(TotalZ)*SinA*SinB*SinG-(float)(PosY)*CosB*SinG);
BodyIKPosY = PosY - (int)((float)(TotalX)*SinA*SinG - (float)(TotalX)*CosA*CosG*SinB + (float)(TotalZ)*CosA*SinG + (float)(TotalZ)*CosG*SinA*SinB + (float)(PosY)*CosB*CosG);

}

//--------------------------------------------------------------------
//[LEG INVERSE KINEMATICS] Calculates the angles of the tibia and femur for the given position of the feet
//IKFeetPosX - Input position of the Feet X
//IKFeetPosY - Input position of the Feet Y
//IKFeetPosZ - Input Position of the Feet Z
//IKSolution - Output true IF the solution is possible
//IKSolutionWarning - Output true IF the solution is NEARLY possible
//IKSolutionError - Output true IF the solution is NOT possible
//IKFemurAngle - Output Angle of Femur in degrees
//IKTibiaAngle - Output Angle of Tibia in degrees
//IKCoxaAngle - Output Angle of Coxa in degrees
void LegIK( int IKFeetPosX, int IKFeetPosY, int IKFeetPosZ )
{
//Length between the Coxa and Feet
IKFeetPosXZ = (int)(sqrt((IKFeetPosXIKFeetPosX)+(IKFeetPosZIKFeetPosZ)));

//IKSW - Length between shoulder and wrist
IKSW = sqrt((((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-CoxaLength))+(IKFeetPosY*IKFeetPosY)));

//IKA1 - Angle between SW line and the ground in rad
GetBoogTan( IKFeetPosXZ-CoxaLength, IKFeetPosY );
IKA1 = BoogTan;

//IKA2 - ?
IKA2 = acos(((float)((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + (IKSW*IKSW)) / ((float)(2*FemurLength) * IKSW));

//IKFemurAngle
IKFemurAngle = ((int)(((IKA1 + IKA2) * 180.0) / M_PI)*-1)+90;

//IKTibiaAngle
IKTibiaAngle = (90-(int)(((acos(((float)((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - (IKSW*IKSW)) / (float)(2*FemurLength*TibiaLength)))*180.0) / M_PI)) * -1;

//IKCoxaAngle
GetBoogTan( IKFeetPosZ, IKFeetPosX );
IKCoxaAngle = (int)((BoogTan*180.0) / M_PI);

//Set the Solution quality
if ( IKSW < (float)(FemurLength+TibiaLength-30)) {
	IKSolution = true;
}
else {
	if ( IKSW < (float)(FemurLength+TibiaLength) )
		IKSolutionWarning = true;
	else
		IKSolutionError = true;
}

}

//--------------------------------------------------------------------
//[CHECK ANGLES] Checks the mechanical limits of the servos
void CheckAngles()
{
RFCoxaAngle = min(max(RFCoxaAngle, RFCoxa_MIN), RFCoxa_MAX);
RFFemurAngle = min(max(RFFemurAngle, RFFemur_MIN), RFFemur_MAX);
RFTibiaAngle = min(max(RFTibiaAngle, RFTibia_MIN), RFTibia_MAX);

RMCoxaAngle  = min(max(RMCoxaAngle, RMCoxa_MIN), RMCoxa_MAX);
RMFemurAngle = min(max(RMFemurAngle, RMFemur_MIN), RMFemur_MAX);
RMTibiaAngle = min(max(RMTibiaAngle, RMTibia_MIN), RMTibia_MAX);

RRCoxaAngle  = min(max(RRCoxaAngle, RRCoxa_MIN), RRCoxa_MAX);
RRFemurAngle = min(max(RRFemurAngle, RRFemur_MIN), RRFemur_MAX);
RRTibiaAngle = min(max(RRTibiaAngle, RRTibia_MIN), RRTibia_MAX);

LFCoxaAngle  = min(max(LFCoxaAngle, LFCoxa_MIN), LFCoxa_MAX);
LFFemurAngle = min(max(LFFemurAngle, LFFemur_MIN), LFFemur_MAX);
LFTibiaAngle = min(max(LFTibiaAngle, LFTibia_MIN), LFTibia_MAX);

LMCoxaAngle  = min(max(LMCoxaAngle, LMCoxa_MIN), LMCoxa_MAX);
LMFemurAngle = min(max(LMFemurAngle, LMFemur_MIN), LMFemur_MAX);
LMTibiaAngle = min(max(LMTibiaAngle, LMTibia_MIN), LMTibia_MAX);

LRCoxaAngle  = min(max(LRCoxaAngle, LRCoxa_MIN), LRCoxa_MAX);
LRFemurAngle = min(max(LRFemurAngle, LRFemur_MIN), LRFemur_MAX);
LRTibiaAngle = min(max(LRTibiaAngle, LRTibia_MIN), LRTibia_MAX);

}

//--------------------------------------------------------------------
//[SERVO DRIVER] Updates the positions of the servos
void ServoDriver()
{
char Serout[500]={0};
int x = 0;

gotoxy(3,7); cout << "Tibia   Femur   Coxa  L|R  Coxa   Femur   Tibia";

//Front Right leg
x = max(min((int)((-RFCoxaAngle +90)/0.10588238)+650, 2500),500);
gotoxy(26, 8); cout << "F   "  << x << "  ";
sprintf(Serout, "#%dP%d", RFCoxaPin, x);

x = max(min((int)((-RFFemurAngle+90)/0.10588238)+650, 2500),500);
gotoxy(37, 8); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RFFemurPin, x);

x = max(min((int)((-RFTibiaAngle+90)/0.10588238)+650, 2500),500);
gotoxy(45, 8); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RFTibiaPin, x);

//Middle Right leg
x = max(min((int)((-RMCoxaAngle +90)/0.10588238)+650, 2500),500);
gotoxy(26, 9); cout << "M   " << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RMCoxaPin, x);

x = max(min((int)((-RMFemurAngle+90)/0.10588238)+650, 2500),500);
gotoxy(37, 9); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RMFemurPin, x);

x = max(min((int)((-RMTibiaAngle+90)/0.10588238)+650, 2500),500);
gotoxy(45, 9); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RMTibiaPin, x);

//Rear Right leg
x = max(min((int)((-RRCoxaAngle +90)/0.10588238)+650, 2500),500);
gotoxy(26,10); cout << "R " << x << " ";
sprintf(Serout, “%s #%dP%d”, Serout, RRCoxaPin, x);

x = max(min((int)((-RRFemurAngle+90)/0.10588238)+650, 2500),500);
gotoxy(37,10); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RRFemurPin, x);

x = max(min((int)((-RRTibiaAngle+90)/0.10588238)+650, 2500),500);
gotoxy(45,10); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, RRTibiaPin, x);

//Front Left leg
x = max(min((int)((LFCoxaAngle +90)/0.10588238)+650, 2500),500);
gotoxy(19, 8); cout << x << " ";
sprintf(Serout, “%s #%dP%d”, Serout, LFCoxaPin, x);

x = max(min((int)((LFFemurAngle+90)/0.10588238)+650, 2500),500);
gotoxy(11, 8); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, LFFemurPin, x);

x = max(min((int)((LFTibiaAngle+90)/0.10588238)+650, 2500),500);
gotoxy( 3, 8); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, LFTibiaPin, x);

//Middle Left leg
x = max(min((int)((LMCoxaAngle +90)/0.10588238)+650, 2500),500);
gotoxy(19, 9); cout << x << " ";
sprintf(Serout, “%s #%dP%d”, Serout, LMCoxaPin, x);

x = max(min((int)((LMFemurAngle+90)/0.10588238)+650, 2500),500);
gotoxy(11, 9); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, LMFemurPin, x);

x = max(min((int)((LMTibiaAngle+90)/0.10588238)+650, 2500),500);
gotoxy( 3, 9); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, LMTibiaPin, x);

//Rear Left leg
x = max(min((int)((LRCoxaAngle +90)/0.10588238)+650, 2500),500);
gotoxy(19,10); cout << x << " ";
sprintf(Serout, “%s #%dP%d”, Serout, LRCoxaPin, x);

x = max(min((int)((LRFemurAngle+90)/0.10588238)+650, 2500),500);
gotoxy(11,10); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, LRFemurPin, x);

x = max(min((int)((LRTibiaAngle+90)/0.10588238)+650, 2500),500);
gotoxy( 3,10); cout << x << "  ";
sprintf(Serout, "%s #%dP%d", Serout, LRTibiaPin, x);

//Send
gotoxy(0,15); cout << “Timer:” << SSCTime;
sprintf(Serout, “%s T%d\r”, Serout, SSCTime);

// write to serial
write(ser_fd, &Serout, sizeof(Serout));

PrevSSCTime = SSCTime;
}

//--------------------------------------------------------------------
//[FREE SERVOS] Frees all the servos
void FreeServos()
{
char Serout[500]={0}, temp[10]={0};
for ( int x=0; x <= 31; x++ )
{
sprintf(temp, "#%dP0 ", x);
sprintf(Serout, “%s%s”, Serout, temp);
}
sprintf(temp, “T200”);
sprintf(Serout, “%s%s\r”, Serout, temp);
//gotoxy(0,30); cout << Serout;
write(ser_fd, &Serout, sizeof(Serout));
}

//--------------------------------------------------------------------
[/code]

Thanks for posting the code!

'Been a while since I’ve seen a gotoxy!

I look forward to studying it.

The mill pix is a little curious, what kind is it? Reminds me of a router, but it’s much more.

Alan KM6VV

Hi,

Looks like an awesome hexapod project! What are the dimensions (length) of the femur and tibia part? Oh wait, I found it in your code. Femur = 120mm, Tibia = 130mm. You are going to get good ground clearance and thats a good thing for outdoor walking. What made you change the tibia design to a inward curve? I’m just asking since that was my first atempt of making a tibia part for Phoenix.

Your coxa design looks very solid!

Keep up the good work, looking forward to see a picture of it when you are finished.

Your new brass bushings to replace bearings are interesting! you show one on a 5980SG servo which I take has a threaded insert on the backside of the case? What about the MG645 servos?

Again, Nice design of parts!

Alan KM6VV

It’s a router but the table moves on the x-axis and not the portal. So it has a better accuracy.

  • One of my targets is to walk the two steps between garden and house. I think he will have more grip on the way upstairs.
  • With this design the bot needs less space in offline mode.
  • The tibia and femur fits in design

The Brass axis is mounted to this insert. In fact that the 645MG don’t have this insert I’ve clued the axis to the bottom side of the coxa.

file-server.servemp3.com/download/huefte_gegenlager_small.jpg

EDIT:
I’ve just updated the the source code earlier in this thread:

  • I forget to translate some commends
  • some useless vars are removed
  • gotoxy() has updated with a shorter version

Can you explain that a little more? Because I found that the inward curved tibias required more space when the legs needed to be lifted close to and over an obstacle.

Thanks for the update in the code. I checked out the ARM9, and it’s an interesting development board, although I’d like to stay with my PICs. It has a limited LCD display, 'tho. I’ll see how your code compiles on the PICs I’m using. I do have an ARM7 board that I designed, Might have to re-think using that board as well.

Yes, I should have guessed that you’d simply incorporate the bearing into the coxa. Good job!

Alan KM6VV

Fantastic project!

Thanks for sharing the code, looks like a nice clean port over to C.

Love the design of your bot, great work.

With the curve in the coxa and the femur parts they can get very close.
But pictures says more than words:

http://file-server.servemp3.com/download/motion1.jpg
http://file-server.servemp3.com/download/motion2.jpg

Hi Chaosmann,

Great design! I see you’ve got some great tools to play with :wink: I like the coxa design. Are you planning on adding a head and tail?

Any information about the price of the controller you’re using?

Let me know if there are any questions about the code :wink:

Xan

Thanks for the comments!

The tools are realy great. We’re able to work with an accuracy from 1/100 mm :smiley: I can’t thanks Karl-Kristian enough!

The head is in planing, but until I’ve find the right sensors, I can’t build it.
I want a camera, a compass, 3-axis gyro and acceleration sensors and a gps as minimum (I want to realize the terrain adaption on this way).
A tail isn’t planed yet, but if I need more space for elektronic, maybe in the future.

The controller costs about 135,- € (Link)
Unfortunately it’s a little to big to build it into the bot, so it will stay in the remote.

About the Code… what’s about the state of V2? :wink:

Hi. We already chatted via PM. :slight_smile:

This board is about 72 Euros including tax and has almost the same specs (in my PM, I wrote 90 Euros, but that was the FPGA board actually).

A head sounds good, I’m playing around with ideas for a pan/tilt head for my 'quad. I have an GP2D12 IR and a SRF08 Ultrasonic Ranger that would be interesting to put in a head. I also bought a TLS1401 line imaging sensor:

parallax.com/Store/Sensors/ObjectDetection/tabid/176/CategoryID/51/List/0/Level/a/ProductID/566/Default.aspx

Which might make an interesting sensor. I have accelerometers and a compass as well, but it might be a while before I get even SOME of these implemented!

Alan KM6VV

Ok, I see what you mean.
What I meant was that this design would make it hard to place the tars (foot) close to an obstacle (start of the stair) when the femur are in the 0 deg position (horisontal).

Anyway, don’t understand me wrong I find your design very cool and interesting. I envy you those beefy servos! :wink:

It would be a challenge to make a hexapod using standard 645 servos beeing able to have the same ground clearance as yours.

Looking forward to see it in action!

It’s alive :slight_smile:

There is still a lot of work to do on the femur and the electronics, but it still can take a walk.
The power supply currently runs o on a 40 A DC power supply

Also the source code has of course changed a little bit:
Download

Nice work! Great to see it moving. It reminds me a bit of a crab with those tibias :wink: One thing that surprised was the sound of the 5980SG servos. In a very positive way though. I was expecting the irritating high pitch sound, but from the video they didn’t sound bad at all. Can you confirm that?