ATmega328 Conversion of Phoenix Atom Source Problem

Help & Support Apps & Software
1

Hi,

I am currently converting the Phoenix Control source by : Jeroen Janssen (aka Xan) to be used on an ATmega328 but i dont know where but it seams that some method doesn’t work like they should and since it is my first project including inverse kinematic i’m lost and cant figure out what is working or not.

Main Prog

[code]/*Project Lynxmotion Phoenix
*Description: Phoenix software
*Software version: V0.1
*Date: 12-03-2010
*Programmer : Marc-Andre Ferland (aka Madrang)
*Original source : Jeroen Janssen (aka Xan)
*
Hardware setup: Arduino, SSC32 V2/

#include <NewSoftSerial.h>
#include “Struct.h”

NewSoftSerial mySerial(5, 4);

//Constants
const boolean DOWN = false;
const boolean UP = true;
const char Cr = 13;

#define _DEBUG true

#define c1DEC 10
#define c2DEC 100
#define c4DEC 10000
#define c6DEC 1000000

#define cRR 0
#define cRM 1
#define cRF 2
#define cLR 3
#define cLM 4
#define cLF 5

//Tables
/*ArcCosinus Table
*Table build in to 3 part to get higher accuracy near cos = 1.
The biggest error is near cos = 1 and has a biggest value of 30.012098rad = 0.521 deg.
*- Cos 0 to 0.9 is done by steps of 0.0079 rad. (1/127)
*- Cos 0.9 to 0.99 is done by steps of 0.0008 rad (0.1/127)
- Cos 0.99 to 1 is done by step of 0.0002 rad (0.01/64)
//Since the tables are overlapping the full range of 127+127+64 is not necessary. Total bytes: 277/
const byte GetACos] = {
255,254,252,251,250,249,247,246,245,243,242,241,240,238,237,236,234,233,232,231,229,228,227,225,
224,223,221,220,219,217,216,215,214,212,211,210,208,207,206,204,203,201,200,199,197,196,195,193,
192,190,189,188,186,185,183,182,181,179,178,176,175,173,172,170,169,167,166,164,163,161,160,158,
157,155,154,152,150,149,147,146,144,142,141,139,137,135,134,132,130,128,127,125,123,121,119,117,
115,113,111,109,107,105,103,101,98,96,94,92,89,87,84,81,79,76,73,73,73,72,72,72,71,71,71,70,70,
70,70,69,69,69,68,68,68,67,67,67,66,66,66,65,65,65,64,64,64,63,63,63,62,62,62,61,61,61,60,60,59,
59,59,58,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,47,
46,46,45,45,44,44,43,43,42,42,41,41,40,40,39,39,38,37,37,36,36,35,34,34,33,33,32,31,31,30,29,28,
28,27,26,25,24,23,23,23,23,22,22,22,22,21,21,21,21,20,20,20,19,19,19,19,18,18,18,17,17,17,17,16,
16,16,15,15,15,14,14,13,13,13,12,12,11,11,10,10,9,9,8,7,6,6,5,3,0};

//Sin table 90 deg, persision 0.5 deg (180 values)
const int GetSin] = {
0, 87, 174, 261, 348, 436, 523, 610, 697, 784, 871, 958, 1045, 1132, 1218, 1305, 1391, 1478, 1564,
1650, 1736, 1822, 1908, 1993, 2079, 2164, 2249, 2334, 2419, 2503, 2588, 2672, 2756, 2840, 2923, 3007,
3090, 3173, 3255, 3338, 3420, 3502, 3583, 3665, 3746, 3826, 3907, 3987, 4067, 4146, 4226, 4305, 4383,
4461, 4539, 4617, 4694, 4771, 4848, 4924, 4999, 5075, 5150, 5224, 5299, 5372, 5446, 5519, 5591, 5664,
5735, 5807, 5877, 5948, 6018, 6087, 6156, 6225, 6293, 6360, 6427, 6494, 6560, 6626, 6691, 6755, 6819,
6883, 6946, 7009, 7071, 7132, 7193, 7253, 7313, 7372, 7431, 7489, 7547, 7604, 7660, 7716, 7771, 7826,
7880, 7933, 7986, 8038, 8090, 8141, 8191, 8241, 8290, 8338, 8386, 8433, 8480, 8526, 8571, 8616, 8660,
8703, 8746, 8788, 8829, 8870, 8910, 8949, 8987, 9025, 9063, 9099, 9135, 9170, 9205, 9238, 9271, 9304,
9335, 9366, 9396, 9426, 9455, 9483, 9510, 9537, 9563, 9588, 9612, 9636, 9659, 9681, 9702, 9723, 9743,
9762, 9781, 9799, 9816, 9832, 9848, 9862, 9876, 9890, 9902, 9914, 9925, 9935, 9945, 9953, 9961, 9969,
9975, 9981, 9986, 9990, 9993, 9996, 9998, 9999, 10000};

//Build tables for Leg configuration like I/O and MIN/MAX values to easy access values using a FOR loop
//Constants are still defined as single values in the cfg file to make it easy to read/configure

//SSC Pin numbers
#define cRRCoxaPin 9 //Rear Right leg Hip Horizontal
#define cRRFemurPin 8 //Rear Right leg Hip Vertical
#define cRRTibiaPin 7 //Rear Right leg Knee

#define cRMCoxaPin 12 //Middle Right leg Hip Horizontal
#define cRMFemurPin 11 //Middle Right leg Hip Vertical
#define cRMTibiaPin 10 //Middle Right leg Knee

#define cRFCoxaPin 15 //Front Right leg Hip Horizontal
#define cRFFemurPin 14 //Front Right leg Hip Vertical
#define cRFTibiaPin 13 //Front Right leg Knee

#define cLRCoxaPin 18 //Rear Left leg Hip Horizontal
#define cLRFemurPin 17 //Rear Left leg Hip Vertical
#define cLRTibiaPin 16 //Rear Left leg Knee

#define cLMCoxaPin 21 //Middle Left leg Hip Horizontal
#define cLMFemurPin 20 //Middle Left leg Hip Vertical
#define cLMTibiaPin 19 //Middle Left leg Knee

#define cLFCoxaPin 24 //Front Left leg Hip Horizontal
#define cLFFemurPin 23 //Front Left leg Hip Vertical
#define cLFTibiaPin 22 //Front Left leg Knee

const byte cCoxaPin] = {
cRRCoxaPin, cRMCoxaPin, cRFCoxaPin, cLRCoxaPin, cLMCoxaPin, cLFCoxaPin};
const byte cFemurPin] = {
cRRFemurPin, cRMFemurPin, cRFFemurPin, cLRFemurPin, cLMFemurPin, cLFFemurPin};
const byte cTibiaPin] = {
cRRTibiaPin, cRMTibiaPin, cRFTibiaPin, cLRTibiaPin, cLMTibiaPin, cLFTibiaPin};

//Leg Sizes
#define cCoxaLength 29 //Length of the Coxa [mm]
#define cFemurLength 76 //Length of the Femur [mm]
#define cTibiaLength 106 //Lenght of the Tibia [mm]

//Min / Max values
#define cRRCoxaMin1 -260 //Mechanical limits of the Right Rear Leg, decimals = 1
#define cRRCoxaMax1 740
#define cRRFemurMin1 -1010
#define cRRFemurMax1 950
#define cRRTibiaMin1 -1060
#define cRRTibiaMax1 770

#define cRMCoxaMin1 -530 //Mechanical limits of the Right Middle Leg, decimals = 1
#define cRMCoxaMax1 530
#define cRMFemurMin1 -1010
#define cRMFemurMax1 950
#define cRMTibiaMin1 -1060
#define cRMTibiaMax1 770

#define cRFCoxaMin1 -580 //Mechanical limits of the Right Front Leg, decimals = 1
#define cRFCoxaMax1 740
#define cRFFemurMin1 -1010
#define cRFFemurMax1 950
#define cRFTibiaMin1 -1060
#define cRFTibiaMax1 770

#define cLRCoxaMin1 -740 //Mechanical limits of the Left Rear Leg, decimals = 1
#define cLRCoxaMax1 260
#define cLRFemurMin1 -950
#define cLRFemurMax1 1010
#define cLRTibiaMin1 -770
#define cLRTibiaMax1 1060

#define cLMCoxaMin1 -530 //Mechanical limits of the Left Middle Leg, decimals = 1
#define cLMCoxaMax1 530
#define cLMFemurMin1 -950
#define cLMFemurMax1 1010
#define cLMTibiaMin1 -770
#define cLMTibiaMax1 1060

#define cLFCoxaMin1 -740 //Mechanical limits of the Left Front Leg, decimals = 1
#define cLFCoxaMax1 580
#define cLFFemurMin1 -950
#define cLFFemurMax1 1010
#define cLFTibiaMin1 -770
#define cLFTibiaMax1 1060

const int cCoxaMin1] = {
cRRCoxaMin1, cRMCoxaMin1, cRFCoxaMin1, cLRCoxaMin1, cLMCoxaMin1, cLFCoxaMin1};
const int cCoxaMax1] = {
cRRCoxaMax1, cRMCoxaMax1, cRFCoxaMax1, cLRCoxaMax1, cLMCoxaMax1, cLFCoxaMax1};
const int cFemurMin1] = {
cRRFemurMin1, cRMFemurMin1, cRFFemurMin1, cLRFemurMin1, cLMFemurMin1, cLFFemurMin1};
const int cFemurMax1] = {
cRRFemurMax1, cRMFemurMax1, cRFFemurMax1, cLRFemurMax1, cLMFemurMax1, cLFFemurMax1};
const int cTibiaMin1] = {
cRRTibiaMin1, cRMTibiaMin1, cRFTibiaMin1, cLRTibiaMin1, cLMTibiaMin1, cLFTibiaMin1};
const int cTibiaMax1] = {
cRRTibiaMax1, cRMTibiaMax1, cRFTibiaMax1, cLRTibiaMax1, cLMTibiaMax1, cLFTibiaMax1};

//Body Offsets (distance between the center of the body and the center of the coxa)
#define cRROffsetX -43 //Distance X from center of the body to the Right Rear coxa
#define cRROffsetZ 82 //Distance Z from center of the body to the Right Rear coxa
#define cRMOffsetX -63 //Distance X from center of the body to the Right Middle coxa
#define cRMOffsetZ 0 //Distance Z from center of the body to the Right Middle coxa
#define cRFOffsetX -43 //Distance X from center of the body to the Right Front coxa
#define cRFOffsetZ -82 //Distance Z from center of the body to the Right Front coxa

#define cLROffsetX 43 //Distance X from center of the body to the Left Rear coxa
#define cLROffsetZ 82 //Distance Z from center of the body to the Left Rear coxa
#define cLMOffsetX 63 //Distance X from center of the body to the Left Middle coxa
#define cLMOffsetZ 0 //Distance Z from center of the body to the Left Middle coxa
#define cLFOffsetX 43 //Distance X from center of the body to the Left Front coxa
#define cLFOffsetZ -82 //Distance Z from center of the body to the Left Front coxa

const char cOffsetX] = {
cRROffsetX, cRMOffsetX, cRFOffsetX, cLROffsetX, cLMOffsetX, cLFOffsetX};
const char cOffsetZ] = {
cRROffsetZ, cRMOffsetZ, cRFOffsetZ, cLROffsetZ, cLMOffsetZ, cLFOffsetZ};

//Default leg angle
#define cRRCoxaAngle1 -600 //Default Coxa setup angle, decimals = 1
#define cRMCoxaAngle1 0 //Default Coxa setup angle, decimals = 1
#define cRFCoxaAngle1 600 //Default Coxa setup angle, decimals = 1
#define cLRCoxaAngle1 -600 //Default Coxa setup angle, decimals = 1
#define cLMCoxaAngle1 0 //Default Coxa setup angle, decimals = 1
#define cLFCoxaAngle1 600 //Default Coxa setup angle, decimals = 1

//Start positions for the leg
#define cRRInitPosX 53 //Start positions of the Right Rear leg
#define cRRInitPosY 25
#define cRRInitPosZ 91

#define cRMInitPosX 105 //Start positions of the Right Middle leg
#define cRMInitPosY 25
#define cRMInitPosZ 0

#define cRFInitPosX 53 //Start positions of the Right Front leg
#define cRFInitPosY 25
#define cRFInitPosZ -91

#define cLRInitPosX 53 //Start positions of the Left Rear leg
#define cLRInitPosY 25
#define cLRInitPosZ 91

#define cLMInitPosX 105 //Start positions of the Left Middle leg
#define cLMInitPosY 25
#define cLMInitPosZ 0

#define cLFInitPosX 53 //Start positions of the Left Front leg
#define cLFInitPosY 25
#define cLFInitPosZ -91

const int cInitPosX] = {
cRRInitPosX, cRMInitPosX, cRFInitPosX, cLRInitPosX, cLMInitPosX, cLFInitPosX};
const int cInitPosY] = {
cRRInitPosY, cRMInitPosY, cRFInitPosY, cLRInitPosY, cLMInitPosY, cLFInitPosY};
const int cInitPosZ] = {
cRRInitPosZ, cRMInitPosZ, cRFInitPosZ, cLRInitPosZ, cLMInitPosZ, cLFInitPosZ};

//Remote
#define cTravelDeadZone 4 //The deadzone for the analog input from the remote

//Angles
int CoxaAngle1[6]; //Actual Angle of the horizontal hip, decimals = 1
int FemurAngle1[6]; //Actual Angle of the vertical hip, decimals = 1
int TibiaAngle1[6]; //Actual Angle of the knee, decimals = 1

//Positions single leg control
boolean SLHold; //Single leg control mode

int LegPosX[6]; //Actual X Posion of the Leg
int LegPosY[6]; //Actual Y Posion of the Leg
int LegPosZ[6]; //Actual Z Posion of the Leg

//Inputs
boolean butA;
boolean butB;
boolean butC;

boolean prev_butA;
boolean prev_butB;
boolean prev_butC;

//GP player
boolean GPStart = true; //Start the GP Player
byte GPSeq; //Number of the sequence
byte GPVerData[3]; //Received data to check the SSC Version
boolean GPEnable; //Enables the GP player when the SSC version ends with “GP”

//Outputs
boolean LedA; //Red
boolean LedB; //Green
boolean LedC; //Orange
boolean Eyes; //Eyes output

//GetSinCos / ArcCos
int AngleDeg1; //Input Angle in degrees, decimals = 1
word ABSAngleDeg1; //Absolute value of the Angle in Degrees, decimals = 1
//int AngleRad4; //Output Angle in radials, decimals = 4

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

//Body Inverse Kinematics
char BodyRotX; //Global Input pitch of the body
char BodyRotY; //Global Input rotation of the body
char BodyRotZ; //Global Input roll of the body
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

Axes TotalBal;

//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; //Diagonal direction from Input X and Z
long IKSW2; //Length between Shoulder and Wrist, decimals = 2
long IKA14; //Angle of the line S>W with respect to the ground in radians, decimals = 4
long IKA24; //Angle of the line S>W with respect to the femur in radians, decimals = 4
long Temp1;
long Temp2;
boolean IKSolution; //Output true if the solution is possible
boolean IKSolutionWarning; //Output true if the solution is NEARLY possible
boolean IKSolutionError; //Output true if the solution is NOT possible

//Timing
word wTimerWOverflowCnt; //used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
long lCurrentTime;
long lTimerStart; //Start time of the calculation cycles
long lTimerEnd; //End time of the calculation cycles
byte CycleTime; //Total Cycle time

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

byte InputTimeDelay; //Delay that depends on the input to get the “sneaking” effect
word SpeedControl; //Adjustible Delay

//[GLOABAL]
boolean HexOn; //Switch to turn on Phoenix
boolean Prev_HexOn; //Previous loop state

//[Balance]
boolean BalanceMode;
int TotalTransX;
int TotalTransZ;
int TotalTransY;
int TotalY; //Total Y distance between the center of the body and the feet

//Single Leg Control
byte SelectedLeg;
byte Prev_SelectedLeg;
int SLLegX;
int SLLegY;
int SLLegZ;
boolean AllDown;

//Gait
byte GaitType; //Gait type
byte NomGaitSpeed; //Nominal speed of the gait

byte LegLiftHeight; //Current Travel height
int TravelLengthX; //Current Travel length X
int TravelLengthZ; //Current Travel length Z
int TravelRotationY; //Current Travel Rotation Y

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

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

byte GaitLegNr[6]; //Init position of the leg

byte GaitLegNrIn; //Input Number of the leg

char GaitPosX[6]; //Array containing Relative X position corresponding to the Gait
char GaitPosY[6]; //Array containing Relative Y position corresponding to the Gait
char GaitPosZ[6]; //Array containing Relative Z position corresponding to the Gait
char GaitRotY[6]; //Array containing Relative Y rotation corresponding to the Gait

//Init
void setup(){

Serial.begin(38400);
mySerial.begin(38400);

//Checks SSC version number if it ends with “GP”
//enable the GP player if it does
delay(10);
GPEnable = false;

//serout cSSC_OUT, cSSC_BAUD, “ver”, 13]
//mySerial.print();

//GetSSCVersion:
/*for(byte Index = 0; Index <= 3;Index++){
serin cSSC_IN, cSSC_BAUD, 1000, timeout, [GPVerData(Index)]
Index = (Index+1)//3 ; shift last 3 chars in data
}

//timeout:
if((GPVerData(0) + GPVerData(1) + GPVerData(2)) = 164)//Check if the last 3 chars are G(71) P(80) cr(13)
GPEnable = true;
//else
//sound P9, [40\5000,40\5000];

delay(10);*/

/*
//Turning off all the leds
LedA = 0
LedB = 0
LedC = 0
Eyes = 0
*/

//Stars Init Positions
for(byte LegIndex = 0; LegIndex <= 5; LegIndex++){
LegPosX[LegIndex] = cInitPosX[LegIndex];//Set start positions for each leg
LegPosY[LegIndex] = cInitPosY[LegIndex];
LegPosZ[LegIndex] = cInitPosZ[LegIndex];
}

//Single leg control. Make sure no leg is selected
SelectedLeg = 255; //No Leg selected
Prev_SelectedLeg = 255;

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

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

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

//Initialize Controller
//InitController();

//SSC
SSCTime = 150;
HexOn = false;

///////////////////////DEBUG
HexOn = true;
Prev_HexOn = false;
////////////////////////
}

//Main
void loop(){
if(_DEBUG){
delay(1000);
IKSolution = false;
IKSolutionWarning = false;
IKSolutionError = false;
}

//Start time
lTimerStart = millis();

//Read input
//ControlInput();
//GOSUB ReadButtons //I/O used by the remote
WriteOutputs(); //Write Outputs

//GP Player

//if(GPEnable)
// GPPlayer();

//Single leg control
SingleLegControl();

//Gait
GaitSeq();

//Balance calculations
TotalTransX = 0; //reset values used for calculation of balance
TotalTransZ = 0;
TotalTransY = 0;

if(BalanceMode){
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){ //balance calculations for all Right legs
Axes Pos;

  Pos.X = -LegPosX[LegIndex] + GaitPosX[LegIndex];
  Pos.Y = (LegPosY[LegIndex] - cInitPosY[LegIndex]) + GaitPosY[LegIndex];
  Pos.Z = LegPosZ[LegIndex] + GaitPosZ[LegIndex];
  BalCalcOneLeg(Pos, LegIndex);
}

for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){ //balance calculations for all Left legs
  Axes Pos;

  Pos.X = LegPosX[LegIndex] + GaitPosX[LegIndex];
  Pos.Y = (LegPosY[LegIndex] - cInitPosY[LegIndex]) + GaitPosY[LegIndex];
  Pos.Z = LegPosZ[LegIndex] + GaitPosZ[LegIndex];
  BalCalcOneLeg(Pos, LegIndex);
}
BalanceBody();

}

//Do IK for all Right legs
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){
Axes Pos;

Pos.X = -LegPosX[LegIndex] + BodyPosX + GaitPosX[LegIndex] - TotalTransX;
Pos.Y = LegPosY[LegIndex] + BodyPosY + GaitPosY[LegIndex] - TotalTransY;
Pos.Z = LegPosZ[LegIndex] + BodyPosZ + GaitPosZ[LegIndex] - TotalTransZ;
Axes BodyIKPos = BodyIK(Pos, TotalBal, GaitRotY[LegIndex], LegIndex);

Pos.X = LegPosX[LegIndex] - BodyPosX + BodyIKPos.X - (GaitPosX[LegIndex] - TotalTransX);
Pos.Y = LegPosY[LegIndex] + BodyPosY - BodyIKPos.Y + GaitPosY[LegIndex] - TotalTransY;
Pos.Z = LegPosZ[LegIndex] + BodyPosZ - BodyIKPos.Z + GaitPosZ[LegIndex] - TotalTransZ;
LegIK(Pos, LegIndex);

}

//Do IK for all Left legs
for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){
Axes Pos;

Pos.X = LegPosX[LegIndex] - BodyPosX + GaitPosX[LegIndex] - TotalTransX;
Pos.Y = LegPosY[LegIndex] + BodyPosY + GaitPosY[LegIndex] - TotalTransY;
Pos.Z = LegPosZ[LegIndex] + BodyPosZ + GaitPosZ[LegIndex] - TotalTransZ;
Axes BodyIKPos = BodyIK(Pos, TotalBal, GaitRotY[LegIndex], LegIndex);

Pos.X = LegPosX[LegIndex] + BodyPosX - BodyIKPos.X + GaitPosX[LegIndex] - TotalTransX;
Pos.Y = LegPosY[LegIndex] + BodyPosY - BodyIKPos.Y + GaitPosY[LegIndex] - TotalTransY;
Pos.Z = LegPosZ[LegIndex] + BodyPosZ - BodyIKPos.Z + GaitPosZ[LegIndex] - TotalTransZ;
LegIK(Pos, LegIndex);

}

//Check mechanical limits
CheckAngles();

//Write IK errors to leds
//LedC = IKSolutionWarning;
//LedA = IKSolutionError;

///Drive Servos
if(HexOn){
if(Prev_HexOn == false){
//Sound P9,[60\4000,80\4500,100\5000]
//Eyes = 1
}

//Set SSC time
if(abs(TravelLengthX) > cTravelDeadZone || abs(TravelLengthZ) > cTravelDeadZone || abs(TravelRotationY*2) > cTravelDeadZone){
  SSCTime = NomGaitSpeed + (InputTimeDelay * 2) + SpeedControl;

  //Add aditional delay when Balance mode is on
  if(BalanceMode)
    SSCTime += 100;

}
else SSCTime = 200 + SpeedControl; //Movement speed excl. Walking

//Sync BAP with SSC while walking to ensure the prev is completed before sending the next one
if(GaitPosX[cRF] || GaitPosX[cRM] || GaitPosX[cRR] || GaitPosX[cLF] || GaitPosX[cLM] || GaitPosX[cLR]  ||
  GaitPosY[cRF] || GaitPosY[cRM] || GaitPosY[cRR] || GaitPosY[cLF] || GaitPosY[cLM] || GaitPosY[cLR]  ||
  GaitPosZ[cRF] || GaitPosZ[cRM] || GaitPosZ[cRR] || GaitPosZ[cLF] || GaitPosZ[cLM] || GaitPosZ[cLR]  ||
  GaitRotY[cRF] || GaitRotY[cRM] || GaitRotY[cRR] || GaitRotY[cLF] || GaitRotY[cLM] || GaitRotY[cLR]){

  //Get endtime and calculate wait time
  lTimerEnd = millis();
  CycleTime = (lTimerEnd-lTimerStart)/1000;

  //Wait for previous commands to be completed while walking
  delay(min((PrevSSCTime - CycleTime - 45),1)); //Min 1 ensures that there alway is a value in the pause command
}

ServoDriver();

}
else{

//Turn the bot off
if (Prev_HexOn || !AllDown){
  SSCTime = 600;
  ServoDriver();
  //Sound P9,[100\5000,80\4500,60\4000]      
  delay(600);
}
else{
  FreeServos();
  Eyes = false;
}

}

//Store previous HexOn State
Prev_HexOn = HexOn;

if(_DEBUG){
if(IKSolution)
Serial.println(“IK : Ok”);
if(IKSolutionWarning)
Serial.println(“IK : Warning”);
if(IKSolutionError)
Serial.println(“IK : Error”);
}
}

//Reading input buttons from the ABB
void ReadButtons(){
//input P4
//input P5
//input P6

//prev_butA = butA
//prev_butB = butB
//prev_butC = butC

//butA = IN4
//butB = IN5
//butC = IN6
}

//Updates the state of the leds
void WriteOutputs(){
/* IF ledA = 1 THEN
low p4
ENDIF
IF ledB = 1 THEN
low p5
ENDIF
IF ledC = 1 THEN
low p6
ENDIF*/

/IF Eyes = 0 THEN
low cEyesPin
ELSE
high cEyesPin
ENDIF/
}

/*void GPPlayer(){

byte GPStatSeq;
byte GPStatFromStep;
byte GPStatToStep;
byte GPStatTime;

//Start sequence
if(GPStart){
serout cSSC_OUT, cSSC_BAUD, “PL0SQ”, dec GPSeq, “ONCE”, 13]; //Start sequence

//Wait for GPPlayer to complete sequence
boolean Answer = false;
while(!Answer){
serout cSSC_OUT, cSSC_BAUD, “QPL0”, 13];
serin cSSC_IN, cSSC_BAUD, [GPStatSeq, GPStatFromStep, GPStatToStep, GPStatTime];

if(!(GPStatSeq<>255 | GPStatFromStep<>0 | GPStatToStep<>0 | GPStatTime<>0))
Anwser = true;

GPStart = false;
}
}
}*/

//Single leg control
void SingleLegControl(){

//Check if all legs are down
AllDown = (LegPosY[cRF] == cInitPosY[cRF] && LegPosY[cRM] == cInitPosY[cRM] && LegPosY[cRR] == cInitPosY[cRR] && LegPosY[cLR] == cInitPosY[cLR] && LegPosY[cLM] == cInitPosY[cLM] && LegPosY[cLF] == cInitPosY[cLF]);

if(SelectedLeg >= 0 && SelectedLeg <= 5){
if(SelectedLeg != Prev_SelectedLeg){

  if(AllDown){//Lift leg a bit when it got selected
    LegPosY[SelectedLeg] = cInitPosY[SelectedLeg]-20;
    //Store current status
    Prev_SelectedLeg = SelectedLeg;

  }
  else{ //Return prev leg back to the init position
    LegPosX[Prev_SelectedLeg] = cInitPosX[Prev_SelectedLeg];
    LegPosY[Prev_SelectedLeg] = cInitPosY[Prev_SelectedLeg];
    LegPosZ[Prev_SelectedLeg] = cInitPosZ[Prev_SelectedLeg];
  }

}
else if(!SLHold){
  LegPosY[SelectedLeg] = LegPosY[SelectedLeg] + SLLegY;
  LegPosX[SelectedLeg] = cInitPosX[SelectedLeg] + SLLegX;
  LegPosZ[SelectedLeg] = cInitPosZ[SelectedLeg] + SLLegZ;
}

}
else{//All legs to init position
if(!AllDown){
for(byte LegIndex = 0; LegIndex <= 5;LegIndex++){
LegPosX[LegIndex] = cInitPosX[LegIndex];
LegPosY[LegIndex] = cInitPosY[LegIndex];
LegPosZ[LegIndex] = cInitPosZ[LegIndex];
}
}
if(Prev_SelectedLeg != 255){
Prev_SelectedLeg = 255;
}
}
}

void GaitSelect(){
//Gait selector
if(GaitType == 0){//Ripple Gait 6 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 2;
GaitLegNr[cLM] = 3;
GaitLegNr[cRR] = 4;
GaitLegNr[cLF] = 5;
GaitLegNr[cRM] = 6;

NrLiftedPos = 1;
HalfLiftHeigth = 0;	
TLDivFactor = 4;
StepsInGait = 6;
NomGaitSpeed = 100;

}

if(GaitType == 1){//Ripple Gait 12 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 3;
GaitLegNr[cLM] = 5;
GaitLegNr[cRR] = 7;
GaitLegNr[cLF] = 9;
GaitLegNr[cRM] = 11;

NrLiftedPos = 3;
HalfLiftHeigth = 0;//1
TLDivFactor = 8;
StepsInGait = 12;
NomGaitSpeed = 85;

}

if(GaitType == 2){//Quadripple 9 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 2;
GaitLegNr[cLM] = 4;
GaitLegNr[cRR] = 5;
GaitLegNr[cLF] = 7;
GaitLegNr[cRM] = 8;

NrLiftedPos = 2;
HalfLiftHeigth = 0;	
TLDivFactor = 6;
StepsInGait = 9;
NomGaitSpeed = 100;

}

if(GaitType == 3) {//Tripod 4 steps
GaitLegNr[cLR] = 3;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 1;
GaitLegNr[cRR] = 1;
GaitLegNr[cLF] = 3;
GaitLegNr[cRM] = 3;

NrLiftedPos = 1;
HalfLiftHeigth = 0;
TLDivFactor = 2;
StepsInGait = 4;
NomGaitSpeed = 150;

}

if(GaitType == 4) { //Tripod 6 steps
GaitLegNr[cLR] = 4;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 1;
GaitLegNr[cRR] = 1;
GaitLegNr[cLF] = 4;
GaitLegNr[cRM] = 4;

NrLiftedPos = 2;
HalfLiftHeigth = 0;
TLDivFactor = 4;
StepsInGait = 6;
NomGaitSpeed = 100;

}

if(GaitType == 5){ //Tripod 8 steps
GaitLegNr[cLR] = 5;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 1;
GaitLegNr[cRR] = 1;
GaitLegNr[cLF] = 5;
GaitLegNr[cRM] = 5;

NrLiftedPos = 3;
HalfLiftHeigth = 1;
TLDivFactor = 4;
StepsInGait = 8;
NomGaitSpeed = 85;

}

if(GaitType == 6){ //Wave 12 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 11;
GaitLegNr[cLM] = 3;
GaitLegNr[cRR] = 7;
GaitLegNr[cLF] = 5;
GaitLegNr[cRM] = 9;

NrLiftedPos = 1;
HalfLiftHeigth = 0;
TLDivFactor = 10;
StepsInGait = 12;
NomGaitSpeed = 85;

}

if(GaitType == 7) { //Wave 18 steps
GaitLegNr[cLR] = 4;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 7;
GaitLegNr[cRR] = 13;
GaitLegNr[cLF] = 10;
GaitLegNr[cRM] = 16;

NrLiftedPos = 2;
HalfLiftHeigth = 0;
TLDivFactor = 16;
StepsInGait = 18;
NomGaitSpeed = 85;

}
}

//Gait Sequence
void GaitSeq(){
//Calculate Gait sequence
LastLeg = false;
for(byte LegIndex = 0; LegIndex <= 5; LegIndex++){ // for all legs
if(LegIndex = 5)//last leg
LastLeg = true;
Gait(LegIndex);
}//next leg
}

void Gait (byte GaitCurrentLegNr){

//Check IF the Gait is in motion
GaitInMotion = ((abs(TravelLengthX) > cTravelDeadZone) || (abs(TravelLengthZ) > cTravelDeadZone) || (abs(TravelRotationY) > cTravelDeadZone));

//Clear values under the cTravelDeadZone
if(GaitInMotion == 0){
TravelLengthX = 0;
TravelLengthZ = 0;
TravelRotationY = 0;
}

//Leg middle up position
//Gait in motion -> Gait NOT in motion, return to home position
//Up
if ((GaitInMotion && (NrLiftedPos=1 || NrLiftedPos == 3) && GaitStep == GaitLegNr[GaitCurrentLegNr]) || (!GaitInMotion && GaitStep == GaitLegNr[GaitCurrentLegNr] && ((abs(GaitPosX[GaitCurrentLegNr]) > 2) || (abs(GaitPosZ[GaitCurrentLegNr]) > 2) || (abs(GaitRotY[GaitCurrentLegNr]) > 2)))){
GaitPosX[GaitCurrentLegNr] = 0;
GaitPosY[GaitCurrentLegNr] = -LegLiftHeight;
GaitPosZ[GaitCurrentLegNr] = 0;
GaitRotY[GaitCurrentLegNr] = 0;
}
else{
//Optional Half heigth Rear
if(((NrLiftedPos == 2 && GaitStep == GaitLegNr[GaitCurrentLegNr]) || (NrLiftedPos == 3 && (GaitStep == GaitLegNr[GaitCurrentLegNr] - 1 || GaitStep == GaitLegNr[GaitCurrentLegNr] + (StepsInGait-1)))) && GaitInMotion){
GaitPosX[GaitCurrentLegNr] = -TravelLengthX / 2;
GaitPosY[GaitCurrentLegNr] = -LegLiftHeight / (HalfLiftHeigth + 1);
GaitPosZ[GaitCurrentLegNr] = -TravelLengthZ / 2;
GaitRotY[GaitCurrentLegNr] = -TravelRotationY / 2;
}
else{
//Optional half heigth front
if((NrLiftedPos >= 2) && (GaitStep == GaitLegNr[GaitCurrentLegNr] + 1 || GaitStep == GaitLegNr[GaitCurrentLegNr] - (StepsInGait - 1)) && GaitInMotion){
GaitPosX[GaitCurrentLegNr] = TravelLengthX / 2;
GaitPosY[GaitCurrentLegNr] = -LegLiftHeight / (HalfLiftHeigth + 1);
GaitPosZ[GaitCurrentLegNr] = TravelLengthZ / 2;
GaitRotY[GaitCurrentLegNr] = TravelRotationY / 2;
}
else{
//Leg front down position
if((GaitStep == GaitLegNr[GaitCurrentLegNr] + NrLiftedPos || GaitStep == GaitLegNr[GaitCurrentLegNr] - (StepsInGait - NrLiftedPos)) && GaitPosY[GaitCurrentLegNr] < 0){
GaitPosX[GaitCurrentLegNr] = TravelLengthX/2;
GaitPosZ[GaitCurrentLegNr] = TravelLengthZ/2;
GaitRotY[GaitCurrentLegNr] = TravelRotationY/2;
GaitPosY[GaitCurrentLegNr] = 0;//Only move leg down at once if terrain adaption is turned off

      //Move body forward      
    }
    else{
      GaitPosX[GaitCurrentLegNr] = GaitPosX[GaitCurrentLegNr] - (TravelLengthX/TLDivFactor);
      GaitPosY[GaitCurrentLegNr] = 0;
      GaitPosZ[GaitCurrentLegNr] = GaitPosZ[GaitCurrentLegNr] - (TravelLengthZ/TLDivFactor);
      GaitRotY[GaitCurrentLegNr] = GaitRotY[GaitCurrentLegNr] - (TravelRotationY/TLDivFactor);
    }
  }
}

}

//Advance to the next step
if(LastLeg){ //The last leg in this step
GaitStep = GaitStep + 1;
if(GaitStep > StepsInGait){
GaitStep = 1;
}
}
}

//BalCalcOneLeg
void BalCalcOneLeg(Axes Pos, byte BalLegNr){
//Calculating totals from center of the body to the feet
Axes Total;
Total.Z = cOffsetZ[BalLegNr] + Pos.Z;
Total.X = cOffsetX[BalLegNr] + Pos.X;
Total.Y = 150 + Pos.Y; //using the value 150 to lower the centerpoint of rotation //BodyPosY +

Axes TotalTrans;
TotalTrans.Y = TotalTransY + Pos.Y;
TotalTrans.Z = TotalTransZ + TotalZ;
TotalTrans.X = TotalTransX + TotalX;

int ATan4 = GetATan2(Total.X, Total.Z);

TotalBal.Y += (ATan4*180) / 31415;

ATan4 = GetATan2(Total.X, Total.Y);
TotalBal.Z += ((ATan4*180) / 31415) -90; //Rotate balance circle 90 deg

ATan4 = GetATan2(Total.Z, Total.Y);
TotalBal.X += ((ATan4*180) / 31415) - 90; //Rotate balance circle 90 deg
}

//BalanceBody
void BalanceBody(){
TotalTransZ = TotalTransZ / 6;
TotalTransX = TotalTransX / 6;
TotalTransY = TotalTransY / 6;

if(TotalBal.Y > 0)//Rotate balance circle by +/- 180 deg
TotalBal.Y = TotalBal.Y - 180;
else
TotalBal.Y = TotalBal.Y + 180;

if(TotalBal.Z < -180)//Compensate for extreme balance positions that causes owerflow
TotalBal.Z = TotalBal.Z + 360;

if(TotalBal.X < -180)//Compensate for extreme balance positions that causes owerflow
TotalBal.X = TotalBal.X + 360;

//Balance rotation
TotalBal.X = -TotalBal.X / 6;
TotalBal.Y = -TotalBal.Y / 6;
TotalBal.Z = TotalBal.Z / 6;
}

//Get the sinus and cosinus from the angle +/- multiple circles
CosSin GetSinCos(int AngleDeg1){
CosSin Result;
//Get the absolute value of AngleDeg
if(AngleDeg1 < 0)
ABSAngleDeg1 = AngleDeg1 *-1;
else
ABSAngleDeg1 = AngleDeg1;

//Shift rotation to a full circle of 360 deg -> AngleDeg : 360
if(AngleDeg1 < 0)//Negative values
AngleDeg1 = 3600 - (ABSAngleDeg1 - (3600 * (ABSAngleDeg1 / 3600)));
else//Positive values
AngleDeg1 = ABSAngleDeg1 - (3600 * (ABSAngleDeg1 / 3600));

if(AngleDeg1 >= 0 && AngleDeg1 <= 900){// 0 to 90 deg
Result.Sin4 = GetSin[AngleDeg1 / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = GetSin(900 - AngleDeg1) / 5];
}
else if (AngleDeg1>900 && AngleDeg1<=1800){// 90 to 180 deg
Result.Sin4 = GetSin(900 - (AngleDeg1 - 900)) / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = -GetSin(AngleDeg1 - 900) / 5];
}
else if (AngleDeg1 > 1800 && AngleDeg1 <= 2700){// 180 to 270 deg
Result.Sin4 = -GetSin(AngleDeg1 - 1800) / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = -GetSin(2700 - AngleDeg1) / 5];
}
else if (AngleDeg1 > 2700 && AngleDeg1 <= 3600){// 270 to 360 deg
Result.Sin4 = -GetSin(3600 - AngleDeg1) / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = GetSin(AngleDeg1 - 2700) / 5];
}
return Result;
}

//Get the sinus and cosinus from the angle +/- multiple circles
//AngleRad4 - Output Angle in AngleRad4
int GetArcCos(int Cos4){
//Check for negative value
boolean NegativeValue = false;
if(Cos4 < 0){
Cos4 = -Cos4;
NegativeValue = true;
}

//Limit Cos to his maximal value
Cos4 = max(Cos4, c4DEC);

int AngleRad4;

if(Cos4 >= 0 && Cos4 < 9000){
AngleRad4 = GetACos[Cos4 / 79]; //79=table resolution (1/127)
AngleRad4 = AngleRad4 * 616 / c1DEC; //616=acos resolution (pi/2/255)

}
else if (Cos4 >= 9000 && Cos4 < 9900){
AngleRad4 = GetACos(Cos4 - 9000) / 8 + 114]; //8=table resolution (0.1/127), 114 start address 2nd bytetable range
AngleRad4 = AngleRad4 * 616 / c1DEC; //616=acos resolution (pi/2/255)

}
else if(Cos4 >= 9900 && Cos4 <= 10000){
AngleRad4 = GetACos(Cos4 - 9900) / 2 + 227]; //2=table resolution (0.01/64), 227 start address 3rd bytetable range
AngleRad4 = AngleRad4 * 616 / c1DEC; //616=acos resolution (pi/2/255)
}

//Add negative sign
if(NegativeValue)
AngleRad4 = 31416 - AngleRad4;

return AngleRad4;
}

int XYhyp2(int AtanX, int AtanY){
return sq((AtanXAtanXc4DEC) + (AtanYAtanYc4DEC));
}

//Simplyfied ArcTan2 function based on fixed point ArcCos
//ArcTan4 - Output ARCTAN2(X/Y)
//XYhyp2 - Output presenting Hypotenuse of X and Y
int GetATan2(int AtanX, int AtanY){
int AngleRad = GetArcCos(AtanX*c6DEC / XYhyp2(AtanX, AtanY));

int Atan4 = AngleRad * (AtanY/abs(AtanY));//Add sign
return Atan4;
}

//[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
//BodyIKPosX - Output Position X of feet with Rotation
//BodyIKPosY - Output Position Y of feet with Rotation
//BodyIKPosZ - Output Position Z of feet with Rotation
Axes BodyIK (Axes Pos, Axes TotalBal, char RotationY, byte BodyIKLeg){

//Calculating totals from center of the body to the feet
TotalZ = cOffsetZ[BodyIKLeg] + Pos.Z;
TotalX = cOffsetX[BodyIKLeg] + Pos.X;
//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:
CosSin G = GetSinCos((BodyRotX + TotalBal.X) * c1DEC);
CosSin B = GetSinCos((BodyRotZ + TotalBal.Z) * c1DEC);
CosSin A = GetSinCos((BodyRotY + RotationY + TotalBal.Y) * c1DEC);

//Calcualtion of rotation matrix:
//BodyIKPosX = TotalX - (TotalXCosACosB - TotalZCosBSinA + PosYSinB)
//BodyIKPosZ = TotalZ - (TotalXCosGSinA + TotalXCosASinBSinG + TotalZCosACosG - TotalZSinASinBSinG - PosYCosBSinG)
//BodyIKPosY = PosY - (TotalXSinASinG - TotalXCosACosGSinB + TotalZCosASinG + TotalZCosGSinASinB + PosYCosB*CosG)
Axes BodyIKPos;
BodyIKPos.X = ((TotalX * c2DEC - ( TotalX * c2DEC * A.Cos4 / c4DEC * B.Cos4 / c4DEC - TotalZ * c2DEC * B.Cos4 / c4DEC * A.Sin4 / c4DEC + Pos.Y * c2DEC * B.Sin4 / c4DEC)) / c2DEC);
BodyIKPos.Y = ((Pos.Y * c2DEC - ( TotalX * c2DEC * A.Sin4 / c4DEC * G.Sin4 / c4DEC - TotalX * c2DEC * A.Cos4 / c4DEC * G.Cos4 / c4DEC * B.Sin4 / c4DEC + TotalZ * c2DEC * A.Cos4 / c4DEC * G.Sin4 / c4DEC + TotalZ * c2DEC * G.Cos4 / c4DEC * A.Sin4 / c4DEC * B.Sin4 / c4DEC + Pos.Y * c2DEC * B.Cos4 / c4DEC * G.Cos4 / c4DEC )) / c2DEC);
BodyIKPos.Z = ((TotalZ * c2DEC - ( TotalX * c2DEC * G.Cos4 / c4DEC * A.Sin4 / c4DEC + TotalX * c2DEC * A.Cos4 / c4DEC * B.Sin4 / c4DEC * G.Sin4 / c4DEC + TotalZ * c2DEC * A.Cos4 / c4DEC * G.Cos4 / c4DEC - TotalZ * c2DEC * A.Sin4 / c4DEC * B.Sin4 / c4DEC * G.Sin4 / c4DEC - Pos.Y * c2DEC * B.Cos4 / c4DEC * G.Sin4 / c4DEC )) / c2DEC);
return BodyIKPos;
}

//Calculates the angles of the coxa, femur and tibia for the given position of the feet
//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
//FemurAngle1 - Output Angle of Femur in degrees
//TibiaAngle1 - Output Angle of Tibia in degrees
//CoxaAngle1 - Output Angle of Coxa in degrees
void LegIK(Axes IKFeetPos, byte LegIKLegNr){

//Calculate IKCoxaAngle and IKFeetPosXZ
int ATan4 = GetATan2(IKFeetPos.X, IKFeetPos.Z);
CoxaAngle1[LegIKLegNr] = ((ATan4*180) / 3141) + CoxaAngle1[LegIKLegNr]; // 3141) + cCoxaAngle1[LegIKLegNr];

//Length between the Coxa and tars (foot)
IKFeetPosXZ = XYhyp2(IKFeetPos.X, IKFeetPos.Z) / c2DEC;

//Using GetAtan2 for solving IKA1 and IKSW
//IKA14 - Angle between SW line and the ground in radians
IKA14 = GetATan2(IKFeetPos.Y, IKFeetPosXZ - cCoxaLength);

//IKSW2 - Length between femur axis and tars
IKSW2 = XYhyp2(IKFeetPos.Y, IKFeetPosXZ - cCoxaLength);

//IKA2 - Angle of the line S>W with respect to the femur in radians
Temp1 = (((cFemurLength * cFemurLength) - (cTibiaLength * cTibiaLength)) * c4DEC + (IKSW2 * IKSW2));
Temp2 = ((2 * cFemurLength) * c2DEC * IKSW2);
IKA24 = GetArcCos(Temp1 / (Temp2 / c4DEC));

//IKFemurAngle
FemurAngle1[LegIKLegNr] = -(IKA14 + IKA24) * 180 / 3141 + 900;

//IKTibiaAngle
Temp1 = (((cFemurLength * cFemurLength) + (cTibiaLength * cTibiaLength)) * c4DEC - (IKSW2 * IKSW2));
Temp2 = (2 * cFemurLength * cTibiaLength);
int AngleRad4 = GetArcCos(Temp1 / Temp2);
TibiaAngle1[LegIKLegNr] = -(900-AngleRad4*180/3141);

//Set the Solution quality
if(_DEBUG){
if(IKSW2 < (cFemurLength + cTibiaLength-30) * c2DEC){
IKSolution = true;
}
else{
if(IKSW2 < (cFemurLength + cTibiaLength) * c2DEC){
IKSolutionWarning = true;
}
else{
IKSolutionError = true;
}
}
}
}

//Checks the mechanical limits of the servos
void CheckAngles(){

for(byte LegIndex = 0; LegIndex <= 5; LegIndex++){
CoxaAngle1[LegIndex] = constrain(CoxaAngle1[LegIndex], cCoxaMin1[LegIndex], cCoxaMax1[LegIndex]);
FemurAngle1[LegIndex] = constrain(FemurAngle1[LegIndex], cFemurMin1[LegIndex], cFemurMax1[LegIndex]);
TibiaAngle1[LegIndex] = constrain(TibiaAngle1[LegIndex], cTibiaMin1[LegIndex], cTibiaMax1[LegIndex]);
}
}

//Updates the positions of the servos
void ServoDriver(){

//Update Right Legs
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){
mySerial.print(’#’);
mySerial.print(cCoxaPin[LegIndex],DEC);
mySerial.print(‘P’);
mySerial.print((-CoxaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cFemurPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-FemurAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cTibiaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-TibiaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

}

//Update Left Legs
for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){

mySerial.print('#');
mySerial.print(cCoxaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((CoxaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cFemurPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((FemurAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cTibiaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((TibiaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

}
//Send
mySerial.print(‘T’);
mySerial.print(SSCTime,DEC);
mySerial.print(Cr);

PrevSSCTime = SSCTime;
}

//Frees all the servos
void FreeServos(){
for(byte LegIndex = 0; LegIndex <= 31;LegIndex++){
mySerial.print(’#’);
mySerial.print(LegIndex,DEC);
mySerial.print(“P0”);
}
mySerial.print(“T200”);
mySerial.print(Cr);
}
[/code]

Struct.h

[code]struct CosSin {
int Sin4;
int Cos4;
};

struct Axes {
int X;
int Y;
int Z;
};
[/code]

My hexapod is still an ongoing project but i would like to make the basic part of it work before adding more time to it.
Thx to anybody taking the time to leave a comment.
Madrang

2

Sounds like a fun project.

From your post it sounds like you have some questions? You say some methods do not work. From my quick look though your code it appears like you have commented out the calls to initialize the control input as well as the control input. Maybe I missed something in there were you are force feeding it to maybe try to walk forward or the like, but without it, the bot may not do much. What have you tested? By chance do you also have an Atom Pro? It obviously helps if you can compare the results. What appears to be working or not? Have you tried adding debug outputs to a debug terminal to see what values are set or what is being sent to the SSC-32?

I have a Axon2 (ATMega640), which I implemented the code using Webbotlib for a Brat. With it I do have code for a PS2 controller (likewise XBEE), that probably would not be hard to make run on the ATmega328. Let me know if something like that would help. I think most of that code is up here under the thread: viewtopic.php?f=7&t=5876

Kurt

3

Right now when i try to use it instead of going in it’s “InitPos” it place is legs in a weird position that is clearly indicating that something is wrong(one side all extended and on the other all the way closed). Also i allready got a controller for it with a touch screen and it works but i first need to debug the IK algorithm before trying to control the bot.

4

So have you tried to run it? Conversion looks OK.

Do you have specific questions?

I converted old hexapod code to C (before phoenix).

Alan KM6VV

5

since i dont know what can cause this bug, i dont know where to look. if someone got an idea it would help me alot.

here is the output of it running.

#9P634#8P664#7P621#12P641#11P658#10P622#15P634#14P664#13P621#18P634#17P676#16P649#21P627#20P664#19P649#24P634#23P676#22P649T200
6

This morning i added some debug print to the other serial port and here is the output of the value just before updating the leg position.

Leg No : 0, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 1, Coxa Angle : -189, Femur Angle : -1010, Tibia Angle : -905 Leg No : 2, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 3, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 Leg No : 4, Coxa Angle : -189, Femur Angle : -950, Tibia Angle : -770 Leg No : 5, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 IK : Ok

7

Nice by reading the source and my conversion i got one bug solved (return value of XYhyp2 was supposed to be a long and i used an int)

now i got angles that seams ok

Leg No : 0, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 1, Coxa Angle : -306, Femur Angle : -1010, Tibia Angle : -905 Leg No : 2, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 3, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 Leg No : 4, Coxa Angle : -306, Femur Angle : -950, Tibia Angle : -770 Leg No : 5, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 IK : Ok Leg No : 0, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 1, Coxa Angle : -315, Femur Angle : -1010, Tibia Angle : -905 Leg No : 2, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 3, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 Leg No : 4, Coxa Angle : -315, Femur Angle : -950, Tibia Angle : -770 Leg No : 5, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 IK : Ok Leg No : 0, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 1, Coxa Angle : -324, Femur Angle : -1010, Tibia Angle : -905 Leg No : 2, Coxa Angle : 0, Femur Angle : 950, Tibia Angle : -904 Leg No : 3, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 Leg No : 4, Coxa Angle : -324, Femur Angle : -950, Tibia Angle : -770 Leg No : 5, Coxa Angle : 0, Femur Angle : 963, Tibia Angle : -770 IK : Ok

but the output to the SSC it still

#9P634#8P664#7P621#12P674#11P658#10P622#15P634#14P664#13P621#18P634#17P676#16P649#21P653#20P664#19P649#24P634#23P676#22P649T200 #9P634#8P664#7P621#12P622#11P658#10P622#15P634#14P664#13P621#18P634#17P676#16P649#21P646#20P664#19P649#24P634#23P676#22P649T200 #9P634#8P664#7P621#12P630#11P658#10P622#15P634#14P664#13P621#18P634#17P676#16P649#21P637#20P664#19P649#24P634#23P676#22P649T200

8

I have not looked through the code close enough, but one difference in the C versus this basic is in the case of signed versus unsigned…

That is in C: long l;
Is a signed value to get an unsigned value you need to do something like: unsigned long l;

Where in basic: l var long
Is an unsigned value. To get a signed value you need to: l var slong

Kurt

9

Thx, it seams i made that mistake a lot…

but now it seams there is more to find, as my debug output changed to

IK : Ok IK : Error IK : Ok IK : Ok IK : Error IK : Ok Leg No : 0, Coxa Angle : 0, Femur Angle : 900, Tibia Angle : -900 Leg No : 1, Coxa Angle : -248, Femur Angle : 900, Tibia Angle : -908 Leg No : 2, Coxa Angle : 0, Femur Angle : 900, Tibia Angle : -900 Leg No : 3, Coxa Angle : 0, Femur Angle : 900, Tibia Angle : -770 Leg No : 4, Coxa Angle : -248, Femur Angle : 900, Tibia Angle : -770 Leg No : 5, Coxa Angle : 0, Femur Angle : 900, Tibia Angle : -770

10

Updated code

[code]/*Project Lynxmotion Phoenix
*Description: Phoenix software
*Software version: V0.1
*Date: 12-03-2010
*Programmer : Marc-Andre Ferland (aka Madrang)
*Original source : Jeroen Janssen (aka Xan)
*
Hardware setup: Arduino, SSC32 V2/

#include <NewSoftSerial.h>
#include “Struct.h”

NewSoftSerial mySerial(5, 4);

//Constants
const boolean DOWN = false;
const boolean UP = true;
const char Cr = 13;

#define _DEBUG true

#define c1DEC 10
#define c2DEC 100
#define c4DEC 10000
#define c6DEC 1000000

#define cRR 0
#define cRM 1
#define cRF 2
#define cLR 3
#define cLM 4
#define cLF 5

//Tables
/*ArcCosinus Table
*Table build in to 3 part to get higher accuracy near cos = 1.
The biggest error is near cos = 1 and has a biggest value of 30.012098rad = 0.521 deg.
*- Cos 0 to 0.9 is done by steps of 0.0079 rad. (1/127)
*- Cos 0.9 to 0.99 is done by steps of 0.0008 rad (0.1/127)
- Cos 0.99 to 1 is done by step of 0.0002 rad (0.01/64)
//Since the tables are overlapping the full range of 127+127+64 is not necessary. Total bytes: 277/
const byte GetACos] = {
255,254,252,251,250,249,247,246,245,243,242,241,240,238,237,236,234,233,232,231,229,228,227,225,
224,223,221,220,219,217,216,215,214,212,211,210,208,207,206,204,203,201,200,199,197,196,195,193,
192,190,189,188,186,185,183,182,181,179,178,176,175,173,172,170,169,167,166,164,163,161,160,158,
157,155,154,152,150,149,147,146,144,142,141,139,137,135,134,132,130,128,127,125,123,121,119,117,
115,113,111,109,107,105,103,101,98,96,94,92,89,87,84,81,79,76,73,73,73,72,72,72,71,71,71,70,70,
70,70,69,69,69,68,68,68,67,67,67,66,66,66,65,65,65,64,64,64,63,63,63,62,62,62,61,61,61,60,60,59,
59,59,58,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,47,
46,46,45,45,44,44,43,43,42,42,41,41,40,40,39,39,38,37,37,36,36,35,34,34,33,33,32,31,31,30,29,28,
28,27,26,25,24,23,23,23,23,22,22,22,22,21,21,21,21,20,20,20,19,19,19,19,18,18,18,17,17,17,17,16,
16,16,15,15,15,14,14,13,13,13,12,12,11,11,10,10,9,9,8,7,6,6,5,3,0};

//Sin table 90 deg, persision 0.5 deg (180 values)
const int GetSin] = {
0, 87, 174, 261, 348, 436, 523, 610, 697, 784, 871, 958, 1045, 1132, 1218, 1305, 1391, 1478, 1564,
1650, 1736, 1822, 1908, 1993, 2079, 2164, 2249, 2334, 2419, 2503, 2588, 2672, 2756, 2840, 2923, 3007,
3090, 3173, 3255, 3338, 3420, 3502, 3583, 3665, 3746, 3826, 3907, 3987, 4067, 4146, 4226, 4305, 4383,
4461, 4539, 4617, 4694, 4771, 4848, 4924, 4999, 5075, 5150, 5224, 5299, 5372, 5446, 5519, 5591, 5664,
5735, 5807, 5877, 5948, 6018, 6087, 6156, 6225, 6293, 6360, 6427, 6494, 6560, 6626, 6691, 6755, 6819,
6883, 6946, 7009, 7071, 7132, 7193, 7253, 7313, 7372, 7431, 7489, 7547, 7604, 7660, 7716, 7771, 7826,
7880, 7933, 7986, 8038, 8090, 8141, 8191, 8241, 8290, 8338, 8386, 8433, 8480, 8526, 8571, 8616, 8660,
8703, 8746, 8788, 8829, 8870, 8910, 8949, 8987, 9025, 9063, 9099, 9135, 9170, 9205, 9238, 9271, 9304,
9335, 9366, 9396, 9426, 9455, 9483, 9510, 9537, 9563, 9588, 9612, 9636, 9659, 9681, 9702, 9723, 9743,
9762, 9781, 9799, 9816, 9832, 9848, 9862, 9876, 9890, 9902, 9914, 9925, 9935, 9945, 9953, 9961, 9969,
9975, 9981, 9986, 9990, 9993, 9996, 9998, 9999, 10000};

//Build tables for Leg configuration like I/O and MIN/MAX values to easy access values using a FOR loop
//Constants are still defined as single values in the cfg file to make it easy to read/configure

//SSC Pin numbers
#define cRRCoxaPin 9 //Rear Right leg Hip Horizontal
#define cRRFemurPin 8 //Rear Right leg Hip Vertical
#define cRRTibiaPin 7 //Rear Right leg Knee

#define cRMCoxaPin 12 //Middle Right leg Hip Horizontal
#define cRMFemurPin 11 //Middle Right leg Hip Vertical
#define cRMTibiaPin 10 //Middle Right leg Knee

#define cRFCoxaPin 15 //Front Right leg Hip Horizontal
#define cRFFemurPin 14 //Front Right leg Hip Vertical
#define cRFTibiaPin 13 //Front Right leg Knee

#define cLRCoxaPin 18 //Rear Left leg Hip Horizontal
#define cLRFemurPin 17 //Rear Left leg Hip Vertical
#define cLRTibiaPin 16 //Rear Left leg Knee

#define cLMCoxaPin 21 //Middle Left leg Hip Horizontal
#define cLMFemurPin 20 //Middle Left leg Hip Vertical
#define cLMTibiaPin 19 //Middle Left leg Knee

#define cLFCoxaPin 24 //Front Left leg Hip Horizontal
#define cLFFemurPin 23 //Front Left leg Hip Vertical
#define cLFTibiaPin 22 //Front Left leg Knee

const byte cCoxaPin] = {
cRRCoxaPin, cRMCoxaPin, cRFCoxaPin, cLRCoxaPin, cLMCoxaPin, cLFCoxaPin};
const byte cFemurPin] = {
cRRFemurPin, cRMFemurPin, cRFFemurPin, cLRFemurPin, cLMFemurPin, cLFFemurPin};
const byte cTibiaPin] = {
cRRTibiaPin, cRMTibiaPin, cRFTibiaPin, cLRTibiaPin, cLMTibiaPin, cLFTibiaPin};

//Leg Sizes
#define cCoxaLength 29 //Length of the Coxa [mm]
#define cFemurLength 76 //Length of the Femur [mm]
#define cTibiaLength 106 //Lenght of the Tibia [mm]

//Min / Max values
#define cRRCoxaMin1 -260 //Mechanical limits of the Right Rear Leg, decimals = 1
#define cRRCoxaMax1 740
#define cRRFemurMin1 -1010
#define cRRFemurMax1 950
#define cRRTibiaMin1 -1060
#define cRRTibiaMax1 770

#define cRMCoxaMin1 -530 //Mechanical limits of the Right Middle Leg, decimals = 1
#define cRMCoxaMax1 530
#define cRMFemurMin1 -1010
#define cRMFemurMax1 950
#define cRMTibiaMin1 -1060
#define cRMTibiaMax1 770

#define cRFCoxaMin1 -580 //Mechanical limits of the Right Front Leg, decimals = 1
#define cRFCoxaMax1 740
#define cRFFemurMin1 -1010
#define cRFFemurMax1 950
#define cRFTibiaMin1 -1060
#define cRFTibiaMax1 770

#define cLRCoxaMin1 -740 //Mechanical limits of the Left Rear Leg, decimals = 1
#define cLRCoxaMax1 260
#define cLRFemurMin1 -950
#define cLRFemurMax1 1010
#define cLRTibiaMin1 -770
#define cLRTibiaMax1 1060

#define cLMCoxaMin1 -530 //Mechanical limits of the Left Middle Leg, decimals = 1
#define cLMCoxaMax1 530
#define cLMFemurMin1 -950
#define cLMFemurMax1 1010
#define cLMTibiaMin1 -770
#define cLMTibiaMax1 1060

#define cLFCoxaMin1 -740 //Mechanical limits of the Left Front Leg, decimals = 1
#define cLFCoxaMax1 580
#define cLFFemurMin1 -950
#define cLFFemurMax1 1010
#define cLFTibiaMin1 -770
#define cLFTibiaMax1 1060

const int cCoxaMin1] = {
cRRCoxaMin1, cRMCoxaMin1, cRFCoxaMin1, cLRCoxaMin1, cLMCoxaMin1, cLFCoxaMin1};
const int cCoxaMax1] = {
cRRCoxaMax1, cRMCoxaMax1, cRFCoxaMax1, cLRCoxaMax1, cLMCoxaMax1, cLFCoxaMax1};
const int cFemurMin1] = {
cRRFemurMin1, cRMFemurMin1, cRFFemurMin1, cLRFemurMin1, cLMFemurMin1, cLFFemurMin1};
const int cFemurMax1] = {
cRRFemurMax1, cRMFemurMax1, cRFFemurMax1, cLRFemurMax1, cLMFemurMax1, cLFFemurMax1};
const int cTibiaMin1] = {
cRRTibiaMin1, cRMTibiaMin1, cRFTibiaMin1, cLRTibiaMin1, cLMTibiaMin1, cLFTibiaMin1};
const int cTibiaMax1] = {
cRRTibiaMax1, cRMTibiaMax1, cRFTibiaMax1, cLRTibiaMax1, cLMTibiaMax1, cLFTibiaMax1};

//Body Offsets (distance between the center of the body and the center of the coxa)
#define cRROffsetX -43 //Distance X from center of the body to the Right Rear coxa
#define cRROffsetZ 82 //Distance Z from center of the body to the Right Rear coxa
#define cRMOffsetX -63 //Distance X from center of the body to the Right Middle coxa
#define cRMOffsetZ 0 //Distance Z from center of the body to the Right Middle coxa
#define cRFOffsetX -43 //Distance X from center of the body to the Right Front coxa
#define cRFOffsetZ -82 //Distance Z from center of the body to the Right Front coxa

#define cLROffsetX 43 //Distance X from center of the body to the Left Rear coxa
#define cLROffsetZ 82 //Distance Z from center of the body to the Left Rear coxa
#define cLMOffsetX 63 //Distance X from center of the body to the Left Middle coxa
#define cLMOffsetZ 0 //Distance Z from center of the body to the Left Middle coxa
#define cLFOffsetX 43 //Distance X from center of the body to the Left Front coxa
#define cLFOffsetZ -82 //Distance Z from center of the body to the Left Front coxa

const char cOffsetX] = {
cRROffsetX, cRMOffsetX, cRFOffsetX, cLROffsetX, cLMOffsetX, cLFOffsetX};
const char cOffsetZ] = {
cRROffsetZ, cRMOffsetZ, cRFOffsetZ, cLROffsetZ, cLMOffsetZ, cLFOffsetZ};

//Default leg angle
#define cRRCoxaAngle1 -600 //Default Coxa setup angle, decimals = 1
#define cRMCoxaAngle1 0 //Default Coxa setup angle, decimals = 1
#define cRFCoxaAngle1 600 //Default Coxa setup angle, decimals = 1
#define cLRCoxaAngle1 -600 //Default Coxa setup angle, decimals = 1
#define cLMCoxaAngle1 0 //Default Coxa setup angle, decimals = 1
#define cLFCoxaAngle1 600 //Default Coxa setup angle, decimals = 1

//Start positions for the leg
#define cRRInitPosX 53 //Start positions of the Right Rear leg
#define cRRInitPosY 25
#define cRRInitPosZ 91

#define cRMInitPosX 105 //Start positions of the Right Middle leg
#define cRMInitPosY 25
#define cRMInitPosZ 0

#define cRFInitPosX 53 //Start positions of the Right Front leg
#define cRFInitPosY 25
#define cRFInitPosZ -91

#define cLRInitPosX 53 //Start positions of the Left Rear leg
#define cLRInitPosY 25
#define cLRInitPosZ 91

#define cLMInitPosX 105 //Start positions of the Left Middle leg
#define cLMInitPosY 25
#define cLMInitPosZ 0

#define cLFInitPosX 53 //Start positions of the Left Front leg
#define cLFInitPosY 25
#define cLFInitPosZ -91

const int cInitPosX] = {
cRRInitPosX, cRMInitPosX, cRFInitPosX, cLRInitPosX, cLMInitPosX, cLFInitPosX};
const int cInitPosY] = {
cRRInitPosY, cRMInitPosY, cRFInitPosY, cLRInitPosY, cLMInitPosY, cLFInitPosY};
const int cInitPosZ] = {
cRRInitPosZ, cRMInitPosZ, cRFInitPosZ, cLRInitPosZ, cLMInitPosZ, cLFInitPosZ};

//Remote
#define cTravelDeadZone 4 //The deadzone for the analog input from the remote

//Angles
int CoxaAngle1[6]; //Actual Angle of the horizontal hip, decimals = 1
int FemurAngle1[6]; //Actual Angle of the vertical hip, decimals = 1
int TibiaAngle1[6]; //Actual Angle of the knee, decimals = 1

//Positions single leg control
boolean SLHold; //Single leg control mode

Axes LegPos[6]; //Actual Posion of the Leg

//GP player
boolean GPStart = true; //Start the GP Player
byte GPSeq; //Number of the sequence
byte GPVerData[3]; //Received data to check the SSC Version
boolean GPEnable; //Enables the GP player when the SSC version ends with “GP”

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

//Body Inverse Kinematics
char BodyRotX; //Global Input pitch of the body
char BodyRotY; //Global Input rotation of the body
char BodyRotZ; //Global Input roll of the body
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

Axes TotalBal;

//Timing
word wTimerWOverflowCnt; //used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
unsigned long lCurrentTime;
unsigned long lTimerStart; //Start time of the calculation cycles
unsigned int CycleTime; //Total Cycle time

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

byte InputTimeDelay; //Delay that depends on the input to get the “sneaking” effect
word SpeedControl; //Adjustible Delay

//[GLOABAL]
boolean HexOn; //Switch to turn on Phoenix
boolean Prev_HexOn; //Previous loop state

//[Balance]
boolean BalanceMode;

Axes TotalTrans;

int TotalY; //Total Y distance between the center of the body and the feet

//Single Leg Control
byte SelectedLeg;
byte Prev_SelectedLeg;
Axes SLLeg;

//Gait
byte GaitType; //Gait type
byte NomGaitSpeed; //Nominal speed of the gait

byte LegLiftHeight; //Current Travel height
int TravelLengthX; //Current Travel length X
int TravelLengthZ; //Current Travel length Z
int TravelRotationY; //Current Travel Rotation Y

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

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

byte GaitLegNr[6]; //Init position of the leg

byte GaitLegNrIn; //Input Number of the leg

char GaitPosX[6]; //Array containing Relative X position corresponding to the Gait
char GaitPosY[6]; //Array containing Relative Y position corresponding to the Gait
char GaitPosZ[6]; //Array containing Relative Z position corresponding to the Gait
char GaitRotY[6]; //Array containing Relative Y rotation corresponding to the Gait

//Init
void setup(){

Serial.begin(38400);
mySerial.begin(38400);

//Checks SSC version number if it ends with “GP”
//enable the GP player if it does
delay(10);
GPEnable = false;

//serout cSSC_OUT, cSSC_BAUD, “ver”, 13]
//mySerial.print();

//GetSSCVersion:
/*for(byte Index = 0; Index <= 3;Index++){
serin cSSC_IN, cSSC_BAUD, 1000, timeout, [GPVerData(Index)]
Index = (Index+1)//3 ; shift last 3 chars in data
}

//timeout:
if((GPVerData(0) + GPVerData(1) + GPVerData(2)) = 164)//Check if the last 3 chars are G(71) P(80) cr(13)
GPEnable = true;
//else
//sound P9, [40\5000,40\5000];

delay(10);*/

//Stars Init Positions
for(byte LegIndex = 0; LegIndex <= 5; LegIndex++){
LegPos[LegIndex].X = cInitPosX[LegIndex];//Set start positions for each leg
LegPos[LegIndex].Y = cInitPosY[LegIndex];
LegPos[LegIndex].Z = cInitPosZ[LegIndex];
}

//Single leg control. Make sure no leg is selected
SelectedLeg = 255; //No Leg selected
Prev_SelectedLeg = 255;

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

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

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

//Initialize Controller
//InitController();

//SSC
SSCTime = 150;
HexOn = false;

if(_DEBUG){
HexOn = true;
Prev_HexOn = false;
InputTimeDelay = 128;
SpeedControl = 1000;
}
}

//Main
void loop(){
if(_DEBUG){
delay(1000);
}
//Start time
lTimerStart = millis();

//Read input
//ControlInput();
//ReadButtons(); //I/O used by the remote
WriteOutputs(); //Write Outputs

//GP Player

//if(GPEnable)
// GPPlayer();

//Single leg control
SingleLegControl();

//Gait
GaitSeq();

//Balance calculations
TotalTrans.X = 0; //reset values used for calculation of balance
TotalTrans.Z = 0;
TotalTrans.Y = 0;

if(BalanceMode){
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){ //balance calculations for all Right legs
Axes Pos;

  Pos.X = -LegPos[LegIndex].X + GaitPosX[LegIndex];
  Pos.Y = (LegPos[LegIndex].Y - cInitPosY[LegIndex]) + GaitPosY[LegIndex];
  Pos.Z = LegPos[LegIndex].Z + GaitPosZ[LegIndex];
  BalCalcOneLeg(Pos, LegIndex);
}

for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){ //balance calculations for all Left legs
  Axes Pos;

  Pos.X = LegPos[LegIndex].X + GaitPosX[LegIndex];
  Pos.Y = (LegPos[LegIndex].Y - cInitPosY[LegIndex]) + GaitPosY[LegIndex];
  Pos.Z = LegPos[LegIndex].Z + GaitPosZ[LegIndex];
  BalCalcOneLeg(Pos, LegIndex);
}
BalanceBody();

}

//Do IK for all Right legs
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){
Axes Pos;

Pos.X = -LegPos[LegIndex].X + BodyPosX + GaitPosX[LegIndex] - TotalTrans.X;
Pos.Y = LegPos[LegIndex].Y + BodyPosY + GaitPosY[LegIndex] - TotalTrans.Y;
Pos.Z = LegPos[LegIndex].Z + BodyPosZ + GaitPosZ[LegIndex] - TotalTrans.Z;
Axes BodyIKPos = BodyIK(Pos, TotalBal, GaitRotY[LegIndex], LegIndex);

Pos.X = LegPos[LegIndex].X - BodyPosX + BodyIKPos.X - (GaitPosX[LegIndex] - TotalTrans.X);
Pos.Y = LegPos[LegIndex].Y + BodyPosY - BodyIKPos.Y + GaitPosY[LegIndex] - TotalTrans.Y;
Pos.Z = LegPos[LegIndex].Z + BodyPosZ - BodyIKPos.Z + GaitPosZ[LegIndex] - TotalTrans.Z;
LegIK(Pos, LegIndex);

}

//Do IK for all Left legs
for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){
Axes Pos;

Pos.X = LegPos[LegIndex].X - BodyPosX + GaitPosX[LegIndex] - TotalTrans.X;
Pos.Y = LegPos[LegIndex].Y + BodyPosY + GaitPosY[LegIndex] - TotalTrans.Y;
Pos.Z = LegPos[LegIndex].Z + BodyPosZ + GaitPosZ[LegIndex] - TotalTrans.Z;
Axes BodyIKPos = BodyIK(Pos, TotalBal, GaitRotY[LegIndex], LegIndex);

Pos.X = LegPos[LegIndex].X + BodyPosX - BodyIKPos.X + GaitPosX[LegIndex] - TotalTrans.X;
Pos.Y = LegPos[LegIndex].Y + BodyPosY - BodyIKPos.Y + GaitPosY[LegIndex] - TotalTrans.Y;
Pos.Z = LegPos[LegIndex].Z + BodyPosZ - BodyIKPos.Z + GaitPosZ[LegIndex] - TotalTrans.Z;
LegIK(Pos, LegIndex);

}

//Check mechanical limits
CheckAngles();

///Drive Servos
if(HexOn){
if(Prev_HexOn == false){ //Power-up
}

//Set SSC time
if(abs(TravelLengthX) > cTravelDeadZone || abs(TravelLengthZ) > cTravelDeadZone || abs(TravelRotationY*2) > cTravelDeadZone){
  SSCTime = NomGaitSpeed + (InputTimeDelay * 2) + SpeedControl;

  //Add aditional delay when Balance mode is on
  if(BalanceMode)
    SSCTime += 100;

}
else SSCTime = 200 + SpeedControl; //Movement speed excl. Walking

//Sync BAP with SSC while walking to ensure the prev is completed before sending the next one
if(GaitPosX[cRF] || GaitPosX[cRM] || GaitPosX[cRR] || GaitPosX[cLF] || GaitPosX[cLM] || GaitPosX[cLR]  ||
  GaitPosY[cRF] || GaitPosY[cRM] || GaitPosY[cRR] || GaitPosY[cLF] || GaitPosY[cLM] || GaitPosY[cLR]  ||
  GaitPosZ[cRF] || GaitPosZ[cRM] || GaitPosZ[cRR] || GaitPosZ[cLF] || GaitPosZ[cLM] || GaitPosZ[cLR]  ||
  GaitRotY[cRF] || GaitRotY[cRM] || GaitRotY[cRR] || GaitRotY[cLF] || GaitRotY[cLM] || GaitRotY[cLR]){

  //Get endtime and calculate wait time
  CycleTime = (millis() - lTimerStart); //CycleTime in ms

  //Wait for previous commands to be completed while walking
  delay(constrain((PrevSSCTime - CycleTime) - 45,1,5000)); //constrain ensures that there is alway a valid value for delay()
}

ServoDriver();

}
else{

//Turn the bot off
if (Prev_HexOn || !AllDown()){
  SSCTime = 600;
  ServoDriver();
  delay(600);
}
else{
  FreeServos();
}

}

//Store previous HexOn State
Prev_HexOn = HexOn;

if(_DEBUG){

}
}

//Reading input buttons
void ReadButtons(){
}

//Updates the state of the leds
void WriteOutputs(){
}

/*void GPPlayer(){

byte GPStatSeq;
byte GPStatFromStep;
byte GPStatToStep;
byte GPStatTime;

//Start sequence
if(GPStart){
serout cSSC_OUT, cSSC_BAUD, “PL0SQ”, dec GPSeq, “ONCE”, 13]; //Start sequence

//Wait for GPPlayer to complete sequence
boolean Answer = false;
while(!Answer){
serout cSSC_OUT, cSSC_BAUD, “QPL0”, 13];
serin cSSC_IN, cSSC_BAUD, [GPStatSeq, GPStatFromStep, GPStatToStep, GPStatTime];

if(!(GPStatSeq<>255 | GPStatFromStep<>0 | GPStatToStep<>0 | GPStatTime<>0))
Anwser = true;

GPStart = false;
}
}
}*/

boolean AllDown(){
//Check if all legs are down
return (LegPos[cRF].Y == cInitPosY[cRF] && LegPos[cRM].Y == cInitPosY[cRM] && LegPos[cRR].Y == cInitPosY[cRR] && LegPos[cLR].Y == cInitPosY[cLR] && LegPos[cLM].Y == cInitPosY[cLM] && LegPos[cLF].Y == cInitPosY[cLF]);
}

//Single leg control
void SingleLegControl(){
if(SelectedLeg >= 0 && SelectedLeg <= 5){
if(SelectedLeg != Prev_SelectedLeg){

  if(AllDown()){//Lift leg a bit when it got selected
    LegPos[SelectedLeg].Y = cInitPosY[SelectedLeg]-20;
    //Store current status
    Prev_SelectedLeg = SelectedLeg;

  }
  else{ //Return prev leg back to the init position
    LegPos[Prev_SelectedLeg].X = cInitPosX[Prev_SelectedLeg];
    LegPos[Prev_SelectedLeg].Y = cInitPosY[Prev_SelectedLeg];
    LegPos[Prev_SelectedLeg].Z = cInitPosZ[Prev_SelectedLeg];
  }

}
else if(!SLHold){
  LegPos[SelectedLeg].X = cInitPosX[SelectedLeg] + SLLeg.X;
  LegPos[SelectedLeg].Y += SLLeg.Y;
  LegPos[SelectedLeg].Z = cInitPosZ[SelectedLeg] + SLLeg.Z;
}

}
else{//All legs to init position
if(!AllDown()){
for(byte LegIndex = 0; LegIndex <= 5;LegIndex++){
LegPos[LegIndex].X = cInitPosX[LegIndex];
LegPos[LegIndex].Y = cInitPosY[LegIndex];
LegPos[LegIndex].Z = cInitPosZ[LegIndex];
}
}
if(Prev_SelectedLeg != 255){
Prev_SelectedLeg = 255;
}
}
}

void GaitSelect(){
//Gait selector
if(GaitType == 0){//Ripple Gait 6 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 2;
GaitLegNr[cLM] = 3;
GaitLegNr[cRR] = 4;
GaitLegNr[cLF] = 5;
GaitLegNr[cRM] = 6;

NrLiftedPos = 1;
HalfLiftHeigth = 0;	
TLDivFactor = 4;
StepsInGait = 6;
NomGaitSpeed = 100;

}

if(GaitType == 1){//Ripple Gait 12 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 3;
GaitLegNr[cLM] = 5;
GaitLegNr[cRR] = 7;
GaitLegNr[cLF] = 9;
GaitLegNr[cRM] = 11;

NrLiftedPos = 3;
HalfLiftHeigth = 0;//1
TLDivFactor = 8;
StepsInGait = 12;
NomGaitSpeed = 85;

}

if(GaitType == 2){//Quadripple 9 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 2;
GaitLegNr[cLM] = 4;
GaitLegNr[cRR] = 5;
GaitLegNr[cLF] = 7;
GaitLegNr[cRM] = 8;

NrLiftedPos = 2;
HalfLiftHeigth = 0;	
TLDivFactor = 6;
StepsInGait = 9;
NomGaitSpeed = 100;

}

if(GaitType == 3) {//Tripod 4 steps
GaitLegNr[cLR] = 3;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 1;
GaitLegNr[cRR] = 1;
GaitLegNr[cLF] = 3;
GaitLegNr[cRM] = 3;

NrLiftedPos = 1;
HalfLiftHeigth = 0;
TLDivFactor = 2;
StepsInGait = 4;
NomGaitSpeed = 150;

}

if(GaitType == 4) { //Tripod 6 steps
GaitLegNr[cLR] = 4;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 1;
GaitLegNr[cRR] = 1;
GaitLegNr[cLF] = 4;
GaitLegNr[cRM] = 4;

NrLiftedPos = 2;
HalfLiftHeigth = 0;
TLDivFactor = 4;
StepsInGait = 6;
NomGaitSpeed = 100;

}

if(GaitType == 5){ //Tripod 8 steps
GaitLegNr[cLR] = 5;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 1;
GaitLegNr[cRR] = 1;
GaitLegNr[cLF] = 5;
GaitLegNr[cRM] = 5;

NrLiftedPos = 3;
HalfLiftHeigth = 1;
TLDivFactor = 4;
StepsInGait = 8;
NomGaitSpeed = 85;

}

if(GaitType == 6){ //Wave 12 steps
GaitLegNr[cLR] = 1;
GaitLegNr[cRF] = 11;
GaitLegNr[cLM] = 3;
GaitLegNr[cRR] = 7;
GaitLegNr[cLF] = 5;
GaitLegNr[cRM] = 9;

NrLiftedPos = 1;
HalfLiftHeigth = 0;
TLDivFactor = 10;
StepsInGait = 12;
NomGaitSpeed = 85;

}

if(GaitType == 7) { //Wave 18 steps
GaitLegNr[cLR] = 4;
GaitLegNr[cRF] = 1;
GaitLegNr[cLM] = 7;
GaitLegNr[cRR] = 13;
GaitLegNr[cLF] = 10;
GaitLegNr[cRM] = 16;

NrLiftedPos = 2;
HalfLiftHeigth = 0;
TLDivFactor = 16;
StepsInGait = 18;
NomGaitSpeed = 85;

}
}

//Gait Sequence
void GaitSeq(){
//Calculate Gait sequence
LastLeg = false;
for(byte LegIndex = 0; LegIndex <= 5; LegIndex++){ // for all legs
if(LegIndex = 5)//last leg
LastLeg = true;
Gait(LegIndex);
}//next leg
}

void Gait (byte GaitCurrentLegNr){

//Check IF the Gait is in motion
GaitInMotion = ((abs(TravelLengthX) > cTravelDeadZone) || (abs(TravelLengthZ) > cTravelDeadZone) || (abs(TravelRotationY) > cTravelDeadZone));

//Clear values under the cTravelDeadZone
if(GaitInMotion == 0){
TravelLengthX = 0;
TravelLengthZ = 0;
TravelRotationY = 0;
}

//Leg middle up position
//Gait in motion -> Gait NOT in motion, return to home position
//Up
if ((GaitInMotion && (NrLiftedPos=1 || NrLiftedPos == 3) && GaitStep == GaitLegNr[GaitCurrentLegNr]) || (!GaitInMotion && GaitStep == GaitLegNr[GaitCurrentLegNr] && ((abs(GaitPosX[GaitCurrentLegNr]) > 2) || (abs(GaitPosZ[GaitCurrentLegNr]) > 2) || (abs(GaitRotY[GaitCurrentLegNr]) > 2)))){
GaitPosX[GaitCurrentLegNr] = 0;
GaitPosY[GaitCurrentLegNr] = -LegLiftHeight;
GaitPosZ[GaitCurrentLegNr] = 0;
GaitRotY[GaitCurrentLegNr] = 0;
}
else{
//Optional Half heigth Rear
if(((NrLiftedPos == 2 && GaitStep == GaitLegNr[GaitCurrentLegNr]) || (NrLiftedPos == 3 && (GaitStep == GaitLegNr[GaitCurrentLegNr] - 1 || GaitStep == GaitLegNr[GaitCurrentLegNr] + (StepsInGait-1)))) && GaitInMotion){
GaitPosX[GaitCurrentLegNr] = -TravelLengthX / 2;
GaitPosY[GaitCurrentLegNr] = -LegLiftHeight / (HalfLiftHeigth + 1);
GaitPosZ[GaitCurrentLegNr] = -TravelLengthZ / 2;
GaitRotY[GaitCurrentLegNr] = -TravelRotationY / 2;
}
else{
//Optional half heigth front
if((NrLiftedPos >= 2) && (GaitStep == GaitLegNr[GaitCurrentLegNr] + 1 || GaitStep == GaitLegNr[GaitCurrentLegNr] - (StepsInGait - 1)) && GaitInMotion){
GaitPosX[GaitCurrentLegNr] = TravelLengthX / 2;
GaitPosY[GaitCurrentLegNr] = -LegLiftHeight / (HalfLiftHeigth + 1);
GaitPosZ[GaitCurrentLegNr] = TravelLengthZ / 2;
GaitRotY[GaitCurrentLegNr] = TravelRotationY / 2;
}
else{
//Leg front down position
if((GaitStep == GaitLegNr[GaitCurrentLegNr] + NrLiftedPos || GaitStep == GaitLegNr[GaitCurrentLegNr] - (StepsInGait - NrLiftedPos)) && GaitPosY[GaitCurrentLegNr] < 0){
GaitPosX[GaitCurrentLegNr] = TravelLengthX/2;
GaitPosZ[GaitCurrentLegNr] = TravelLengthZ/2;
GaitRotY[GaitCurrentLegNr] = TravelRotationY/2;
GaitPosY[GaitCurrentLegNr] = 0;//Only move leg down at once if terrain adaption is turned off

      //Move body forward      
    }
    else{
      GaitPosX[GaitCurrentLegNr] = GaitPosX[GaitCurrentLegNr] - (TravelLengthX/TLDivFactor);
      GaitPosY[GaitCurrentLegNr] = 0;
      GaitPosZ[GaitCurrentLegNr] = GaitPosZ[GaitCurrentLegNr] - (TravelLengthZ/TLDivFactor);
      GaitRotY[GaitCurrentLegNr] = GaitRotY[GaitCurrentLegNr] - (TravelRotationY/TLDivFactor);
    }
  }
}

}

//Advance to the next step
if(LastLeg){ //The last leg in this step
GaitStep = GaitStep + 1;
if(GaitStep > StepsInGait){
GaitStep = 1;
}
}
}

//BalCalcOneLeg
void BalCalcOneLeg(Axes Pos, byte BalLegNr){
//Calculating totals from center of the body to the feet
Axes Total;
Total.Z = cOffsetZ[BalLegNr] + Pos.Z;
Total.X = cOffsetX[BalLegNr] + Pos.X;
Total.Y = 150 + Pos.Y; //using the value 150 to lower the centerpoint of rotation //BodyPosY +

TotalTrans.X = TotalTrans.X + TotalX;
TotalTrans.Y = TotalTrans.Y + Pos.Y;
TotalTrans.Z = TotalTrans.Z + TotalZ;

int ATan4 = GetATan2(Total.X, Total.Z);

TotalBal.Y += (ATan4*180) / 31415;

ATan4 = GetATan2(Total.X, Total.Y);
TotalBal.Z += ((ATan4*180) / 31415) -90; //Rotate balance circle 90 deg

ATan4 = GetATan2(Total.Z, Total.Y);
TotalBal.X += ((ATan4*180) / 31415) - 90; //Rotate balance circle 90 deg
}

//BalanceBody
void BalanceBody(){
TotalTrans.Z = TotalTrans.Z / 6;
TotalTrans.X = TotalTrans.X / 6;
TotalTrans.Y = TotalTrans.Y / 6;

if(TotalBal.Y > 0)//Rotate balance circle by +/- 180 deg
TotalBal.Y = TotalBal.Y - 180;
else
TotalBal.Y = TotalBal.Y + 180;

if(TotalBal.Z < -180)//Compensate for extreme balance positions that causes owerflow
TotalBal.Z = TotalBal.Z + 360;

if(TotalBal.X < -180)//Compensate for extreme balance positions that causes owerflow
TotalBal.X = TotalBal.X + 360;

//Balance rotation
TotalBal.X = -TotalBal.X / 6;
TotalBal.Y = -TotalBal.Y / 6;
TotalBal.Z = TotalBal.Z / 6;
}

//Get the sinus and cosinus from the angle +/- multiple circles
CosSin GetSinCos(int AngleDeg1){
CosSin Result;

word ABSAngleDeg1 = abs(AngleDeg1); //Absolute value of the Angle in Degrees, decimals = 1

//Shift rotation to a full circle of 360 deg -> AngleDeg  : 360

if(AngleDeg1 < 0)//Negative values
AngleDeg1 = 3600 - (ABSAngleDeg1 - (3600 * (ABSAngleDeg1 / 3600)));
else//Positive values
AngleDeg1 = ABSAngleDeg1 - (3600 * (ABSAngleDeg1 / 3600));

if(AngleDeg1 >= 0 && AngleDeg1 <= 900){// 0 to 90 deg
Result.Sin4 = GetSin[AngleDeg1 / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = GetSin(900 - AngleDeg1) / 5];
}
else if (AngleDeg1>900 && AngleDeg1<=1800){// 90 to 180 deg
Result.Sin4 = GetSin(900 - (AngleDeg1 - 900)) / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = -GetSin(AngleDeg1 - 900) / 5];
}
else if (AngleDeg1 > 1800 && AngleDeg1 <= 2700){// 180 to 270 deg
Result.Sin4 = -GetSin(AngleDeg1 - 1800) / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = -GetSin(2700 - AngleDeg1) / 5];
}
else if (AngleDeg1 > 2700 && AngleDeg1 <= 3600){// 270 to 360 deg
Result.Sin4 = -GetSin(3600 - AngleDeg1) / 5]; //5 is the presision (0.5) of the table
Result.Cos4 = GetSin(AngleDeg1 - 2700) / 5];
}
return Result;
}

//Get the sinus and cosinus from the angle +/- multiple circles
int GetArcCos(int Cos4){
//Check for negative value
boolean NegativeValue = false;
if(Cos4 < 0){
Cos4 = -Cos4;
NegativeValue = true;
}

//Limit Cos to his maximal value
Cos4 = max(Cos4, c4DEC);

int AngleRad4;

if(Cos4 >= 0 && Cos4 < 9000){
AngleRad4 = GetACos[Cos4 / 79]; //79=table resolution (1/127)
AngleRad4 = AngleRad4 * 616 / c1DEC; //616=acos resolution (pi/2/255)

}
else if (Cos4 >= 9000 && Cos4 < 9900){
AngleRad4 = GetACos(Cos4 - 9000) / 8 + 114]; //8=table resolution (0.1/127), 114 start address 2nd bytetable range
AngleRad4 = AngleRad4 * 616 / c1DEC; //616=acos resolution (pi/2/255)

}
else if(Cos4 >= 9900 && Cos4 <= 10000){
AngleRad4 = GetACos(Cos4 - 9900) / 2 + 227]; //2=table resolution (0.01/64), 227 start address 3rd bytetable range
AngleRad4 = AngleRad4 * 616 / c1DEC; //616=acos resolution (pi/2/255)
}

//Add negative sign
if(NegativeValue)
AngleRad4 = 31416 - AngleRad4;

return AngleRad4;
}

//Output presenting Hypotenuse of X and Y
unsigned long XYhyp2(int AtanX, int AtanY){
return sq((AtanXAtanXc4DEC) + (AtanYAtanYc4DEC));
}

//Simplyfied ArcTan2 function based on fixed point ArcCos
int GetATan2(int AtanX, int AtanY){
int AngleRad = GetArcCos(AtanX*c6DEC / XYhyp2(AtanX, AtanY));
return AngleRad * (AtanY/abs(AtanY));//Add sign
}

//Body inverse kinematics
Axes BodyIK (Axes Pos, Axes TotalBal, char RotationY, byte BodyIKLeg){

//Calculating totals from center of the body to the feet
TotalZ = cOffsetZ[BodyIKLeg] + Pos.Z;
TotalX = cOffsetX[BodyIKLeg] + Pos.X;
//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:
CosSin G = GetSinCos((BodyRotX + TotalBal.X) * c1DEC);
CosSin B = GetSinCos((BodyRotZ + TotalBal.Z) * c1DEC);
CosSin A = GetSinCos((BodyRotY + RotationY + TotalBal.Y) * c1DEC);

//Calcualtion of rotation matrix:
Axes BodyIKPos;
BodyIKPos.X = ((TotalX * c2DEC - ( TotalX * c2DEC * A.Cos4 / c4DEC * B.Cos4 / c4DEC - TotalZ * c2DEC * B.Cos4 / c4DEC * A.Sin4 / c4DEC + Pos.Y * c2DEC * B.Sin4 / c4DEC)) / c2DEC);
BodyIKPos.Y = ((Pos.Y * c2DEC - ( TotalX * c2DEC * A.Sin4 / c4DEC * G.Sin4 / c4DEC - TotalX * c2DEC * A.Cos4 / c4DEC * G.Cos4 / c4DEC * B.Sin4 / c4DEC + TotalZ * c2DEC * A.Cos4 / c4DEC * G.Sin4 / c4DEC + TotalZ * c2DEC * G.Cos4 / c4DEC * A.Sin4 / c4DEC * B.Sin4 / c4DEC + Pos.Y * c2DEC * B.Cos4 / c4DEC * G.Cos4 / c4DEC )) / c2DEC);
BodyIKPos.Z = ((TotalZ * c2DEC - ( TotalX * c2DEC * G.Cos4 / c4DEC * A.Sin4 / c4DEC + TotalX * c2DEC * A.Cos4 / c4DEC * B.Sin4 / c4DEC * G.Sin4 / c4DEC + TotalZ * c2DEC * A.Cos4 / c4DEC * G.Cos4 / c4DEC - TotalZ * c2DEC * A.Sin4 / c4DEC * B.Sin4 / c4DEC * G.Sin4 / c4DEC - Pos.Y * c2DEC * B.Cos4 / c4DEC * G.Sin4 / c4DEC )) / c2DEC);
return BodyIKPos;
}

//Calculates the angles of the coxa, femur and tibia for the given position of the feet
void LegIK(Axes IKFeetPos, byte LegIKLegNr){

//Calculate IKCoxaAngle and IKFeetPosXZ
int ATan4 = GetATan2(IKFeetPos.X, IKFeetPos.Z);
CoxaAngle1[LegIKLegNr] = ((ATan4*180) / 3141) + CoxaAngle1[LegIKLegNr]; // 3141) + cCoxaAngle1[LegIKLegNr];

//Length between the Coxa and tars (foot)
int IKFeetPosXZ = XYhyp2(IKFeetPos.X, IKFeetPos.Z) / c2DEC; //Diagonal direction from Input X and Z

//Using GetAtan2 for solving IKA1 and IKSW
//IKA14 - Angle between SW line and the ground in radians
unsigned long IKA14 = GetATan2(IKFeetPos.Y, IKFeetPosXZ - cCoxaLength); //Angle of the line S>W with respect to the ground in radians, decimals = 4

//IKSW2 - Length between femur axis and tars
unsigned long IKSW2 = XYhyp2(IKFeetPos.Y, IKFeetPosXZ - cCoxaLength); //Length between Shoulder and Wrist, decimals = 2

//IKA2 - Angle of the line S>W with respect to the femur in radians
unsigned long Temp1 = (((cFemurLength * cFemurLength) - (cTibiaLength * cTibiaLength)) * c4DEC + (IKSW2 * IKSW2));
unsigned long Temp2 = ((2 * cFemurLength) * c2DEC * IKSW2);
unsigned long IKA24 = GetArcCos(Temp1 / (Temp2 / c4DEC)); //Angle of the line S>W with respect to the femur in radians, decimals = 4

//IKFemurAngle
FemurAngle1[LegIKLegNr] = -(IKA14 + IKA24) * 180 / 3141 + 900;

//IKTibiaAngle
Temp1 = (((cFemurLength * cFemurLength) + (cTibiaLength * cTibiaLength)) * c4DEC - (IKSW2 * IKSW2));
Temp2 = (2 * cFemurLength * cTibiaLength);
int AngleRad4 = GetArcCos(Temp1 / Temp2);
TibiaAngle1[LegIKLegNr] = -(900-AngleRad4*180/3141);

//Set the Solution quality
if(_DEBUG){
if(IKSW2 < (cFemurLength + cTibiaLength-30) * c2DEC){
Serial.println(“IK : Ok”);
}
else{
if(IKSW2 < (cFemurLength + cTibiaLength) * c2DEC){
Serial.println(“IK : Warning”);
}
else{
Serial.println(“IK : Error”);
}
}
}
}

//Checks the mechanical limits of the servos
void CheckAngles(){
for(byte LegIndex = 0; LegIndex <= 5; LegIndex++){
CoxaAngle1[LegIndex] = constrain(CoxaAngle1[LegIndex], cCoxaMin1[LegIndex], cCoxaMax1[LegIndex]);
FemurAngle1[LegIndex] = constrain(FemurAngle1[LegIndex], cFemurMin1[LegIndex], cFemurMax1[LegIndex]);
TibiaAngle1[LegIndex] = constrain(TibiaAngle1[LegIndex], cTibiaMin1[LegIndex], cTibiaMax1[LegIndex]);
}
}

//Updates the positions of the servos
void ServoDriver(){

//Update Right Legs
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){

if(_DEBUG){
  Serial.print("Leg No : ");
  Serial.print(LegIndex,DEC);
  Serial.print(", Coxa Angle : ");
  Serial.print(CoxaAngle1[LegIndex],DEC);
  Serial.print(", Femur Angle : ");
  Serial.print(FemurAngle1[LegIndex],DEC);
  Serial.print(", Tibia Angle : ");
  Serial.println(TibiaAngle1[LegIndex],DEC);
}

mySerial.print('#');
mySerial.print(cCoxaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-CoxaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cFemurPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-FemurAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cTibiaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-TibiaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

}

//Update Left Legs
for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){

if(_DEBUG){
  Serial.print("Leg No : ");
  Serial.print(LegIndex,DEC);
  Serial.print(", Coxa Angle : ");
  Serial.print(CoxaAngle1[LegIndex],DEC);
  Serial.print(", Femur Angle : ");
  Serial.print(FemurAngle1[LegIndex],DEC);
  Serial.print(", Tibia Angle : ");
  Serial.println(TibiaAngle1[LegIndex],DEC);
}

mySerial.print('#');
mySerial.print(cCoxaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((CoxaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cFemurPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((FemurAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cTibiaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((TibiaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

}
//Send
mySerial.print(‘T’);
mySerial.print(SSCTime,DEC);
mySerial.print(Cr);

PrevSSCTime = SSCTime;
}

//Frees all the servos
void FreeServos(){
for(byte LegIndex = 0; LegIndex <= 31;LegIndex++){
mySerial.print(’#’);
mySerial.print(LegIndex,DEC);
mySerial.print(“P0”);
}
mySerial.print(“T200”);
mySerial.print(Cr);
}
[/code]

Struct.h is still the same…

11

i think right now what would help me is having the output of some function with a given input.

i’m gonna start with :

XYhyp2
GetATan2
GetArcCos
GetSinCos

but the problem is that i don’t have access to a bot board or an atom to run the original code an compare the result.
so if somebody can run these functions and give me the returned result it would help me alot.

12

After some more testing i found 2 bug ans solved one :

XYhyp2 = SQR ((AtanXAtanXc4DEC) + (AtanYAtanYc4DEC))

i tough SQR was square so i used sq instead but figured out that is it SquareRoot. solved by using sqrt instead.

also i got this output

Before CheckAngles() : Leg No : 0, Coxa Angle : -12, Femur Angle : 900, Tibia Angle : -900 Before CheckAngles() : Leg No : 1, Coxa Angle : -10, Femur Angle : 900, Tibia Angle : -900 Before CheckAngles() : Leg No : 2, Coxa Angle : 12, Femur Angle : 900, Tibia Angle : -900 Before CheckAngles() : Leg No : 3, Coxa Angle : -12, Femur Angle : 900, Tibia Angle : -900 Before CheckAngles() : Leg No : 4, Coxa Angle : -10, Femur Angle : 900, Tibia Angle : -900 Before CheckAngles() : Leg No : 5, Coxa Angle : 12, Femur Angle : 900, Tibia Angle : -900 After CheckAngles() : Leg No : 0, Coxa Angle : -12, Femur Angle : 900, Tibia Angle : -900 After CheckAngles() : Leg No : 1, Coxa Angle : -10, Femur Angle : 900, Tibia Angle : -900 After CheckAngles() : Leg No : 2, Coxa Angle : 12, Femur Angle : 900, Tibia Angle : -900 After CheckAngles() : Leg No : 3, Coxa Angle : -12, Femur Angle : 900, Tibia Angle : -770 After CheckAngles() : Leg No : 4, Coxa Angle : -10, Femur Angle : 900, Tibia Angle : -770 After CheckAngles() : Leg No : 5, Coxa Angle : 12, Femur Angle : 900, Tibia Angle : -770

witch seam to indicate that the servos are in the normal range but the SSC it still reciving

#9P650#8P650#7P678#12P648#11P650#10P678#15P678#14P650#13P678#18P678#17P678#16P649#21P620#20P678#19P649#24P650#23P678#22P649T1200

so the bug i cant seams to find is in this part of my code

[code]void ServoDriver(){

//Update Right Legs
for(byte LegIndex = 0; LegIndex <= 2; LegIndex++){

if(_DEBUG){
  Serial.print("After CheckAngles() : ");
  Serial.print("Leg No : ");
  Serial.print(LegIndex,DEC);
  Serial.print(", Coxa Angle : ");
  Serial.print(CoxaAngle1[LegIndex],DEC);
  Serial.print(", Femur Angle : ");
  Serial.print(FemurAngle1[LegIndex],DEC);
  Serial.print(", Tibia Angle : ");
  Serial.println(TibiaAngle1[LegIndex],DEC);
}

mySerial.print('#');
mySerial.print(cCoxaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-CoxaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cFemurPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-FemurAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cTibiaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((-TibiaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

}

//Update Left Legs
for(byte LegIndex = 3; LegIndex <= 5; LegIndex++){

if(_DEBUG){
  Serial.print("After CheckAngles() : ");
  Serial.print("Leg No : ");
  Serial.print(LegIndex,DEC);
  Serial.print(", Coxa Angle : ");
  Serial.print(CoxaAngle1[LegIndex],DEC);
  Serial.print(", Femur Angle : ");
  Serial.print(FemurAngle1[LegIndex],DEC);
  Serial.print(", Tibia Angle : ");
  Serial.println(TibiaAngle1[LegIndex],DEC);
}

mySerial.print('#');
mySerial.print(cCoxaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((CoxaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cFemurPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((FemurAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

mySerial.print('#');
mySerial.print(cTibiaPin[LegIndex],DEC);
mySerial.print('P');
mySerial.print((TibiaAngle1[LegIndex] + 900) * 1000 / 1059 + 650,DEC);

}
//Send
mySerial.print(‘T’);
mySerial.print(SSCTime,DEC);
mySerial.print(Cr);

PrevSSCTime = SSCTime;
}[/code]

13

While looking at this “(CoxaAngle1(LegIndex) +900)*1000/1059+650” in search for a bug. i tought of something the old “(x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min” some kind of scaling function… is it its purpose ? (still im gonna try to use the map function on my platform who’s use is only for scaling, and check the result)

14

After playing around i got this output and solved one more bug.

Map : 1545 Old : 678 Old + Long : 1545

by using this code

Serial.print("Map : "); Serial.print(map(CoxaAngle1[LegIndex],-900,900,650,2350),DEC); Serial.print(" Old : "); Serial.print((CoxaAngle1[LegIndex] +900)*1000/1059+650,DEC); Serial.print(" Old + Long : "); Serial.println(((long)CoxaAngle1[LegIndex] +900)*1000/1059+650,DEC);

i guest on some other line i’m gonna also need to use long to avoid overflow…
and still i can’t compare my result with good data…

15

Also if somebody can explain to me what this does it would be nice as it dosent seam to be usefull, as it alwais output 0 or 3600…

[code]CosSin GetSinCos(int AngleDeg1){
CosSin Result;

word ABSAngleDeg1 = abs(AngleDeg1); //Absolute value of the Angle in Degrees, decimals = 1

//Shift rotation to a full circle of 360 deg -> AngleDeg  : 360

if(AngleDeg1 < 0)//Negative values
AngleDeg1 = 3600 - (ABSAngleDeg1 - (3600 * (ABSAngleDeg1 / 3600)));
else//Positive values
AngleDeg1 = ABSAngleDeg1 - (3600 * (ABSAngleDeg1 / 3600));[/code]

and from the original atom source

;Shift rotation to a full circle of 360 deg -> AngleDeg // 360 IF AngleDeg1 < 0 THEN ;Negative values AngleDeg1 = 3600-(ABSAngleDeg1-(3600*(ABSAngleDeg1/3600))) ELSE ;Positive values AngleDeg1 = ABSAngleDeg1-(3600*(ABSAngleDeg1/3600)) ENDIF

16

still haven figured out the use of the code i posted before and right now this are the value calculated with my code when the hexapod is standing still. (because i haven’t coded the controller yet). and he still can get on his leg as those value are wrong… but witch one… and i don’t know where to look now…

[code]Leg No : 0, X : 53, Y : 25, Z : 91
Leg No : 1, X : 105, Y : 25, Z : 0
Leg No : 2, X : 53, Y : 25, Z : -91
Leg No : 3, X : 53, Y : 25, Z : 91
Leg No : 4, X : 105, Y : 25, Z : 0
Leg No : 5, X : 53, Y : 25, Z : -91

Leg No : 0, Coxa Angle : -260, Femur Angle : 900, Tibia Angle : -900
Leg No : 1, Coxa Angle : -5, Femur Angle : 900, Tibia Angle : -900
Leg No : 2, Coxa Angle : 600, Femur Angle : 900, Tibia Angle : -900
Leg No : 3, Coxa Angle : -600, Femur Angle : 900, Tibia Angle : -770
Leg No : 4, Coxa Angle : -5, Femur Angle : 900, Tibia Angle : -770
Leg No : 5, Coxa Angle : 580, Femur Angle : 900, Tibia Angle : -770
[/code]

#9P1745#8P650#7P1493#12P1504#11P650#10P1490#15P933#14P650#13P1487#18P933#17P2350#16P772#21P1495#20P2350#19P772#24P2047#23P2350#22P772T1200