I’ve already thought about a lab supply. I must see, what my finance minister says 
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 
#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)((BoogTan180.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]
)
