Modifying Phoenix code to work for small hexapod

Ive been working on a small hexapod that i have been trying to run using the phoenix hexapod code.

With help from members of this forum we have made some headway but the topic was on was off topic of wgat i originally started it for so i figured we would make it for the right topic. If that made sense.

So far we have done these things........

Changed the Leg IK measurements to match my hexapod.

Changed the body IK to my bots measurements

Changed the numbers of the feet start positions for my legs.

We also changed the PWM range to match my servos in the servo driver function.

The video below shows how it is walking when i move an analog stick. You can also see me using the triangle and d-pad up\down to raise and lower the bot.

But the controls seem to be reverse on the body lowering/raising. D-pad up brings it down, D-pad down brings it up.

http://youtu.be/eftuerfLkr8

If you need to see my code files please tell me which file you need to see and i will post it as soon as i see the request.

All your code. Or none of it.

Are you sure all the IK parameters are correct? In the video, the leg at the "top" seems to be going the wrong way on its knee joint and maybe one of the others is too.

If up/down are switched, then just switch the connections on your control pot. On an aircraft, pushing forwards is "down" while pushing a mouse "forwards" moves the pointer up the screen. It just depends on your point of view.

What do you mean by all my code or none of it?

The servo at the top is a weak servo that needs to be replaced by fresh one. Ive been at this for a while so its time.

I am sure I changed everything i mentioned above to match my bot.

I guess i should have mentioned im using a PS2 Tx/Rx pair to control the hex wirelessly. I dont think i have a pot.

This has been a big learning experience and i know im still not up to date so i might ask you to elaborate more on some things.

Thank you

We can't debug snippets. 90% of the time that somebody posts a snippet of code that they think has a problem, it turns out that the problem is elsewhere. C coding by beginners has simple mistakes in the declaration of variables, because the beginners don't think it's important, which is why they didn't post the declaration part of their code. QED.

i dont know what needs debugging therefore i wont be posting snippets but whole pages of code. i will have to post some in two posts though since there is a limit.

the first three posts are my config.h

//[CONDITIONAL COMPILING] - COMMENT IF NOT WANTED
// Define other optional compnents to be included or not…

//comment if terminal monitor is not required
#define OPT_TERMINAL_MONITOR

//uncomment the board you want to use
#define BOTBOARDUINO //botboarduino board
//#define BOTBOARD_ARDUINOPROMINI //arduino pro mini on botboard (originally for BasicAtomPro)

//====================================================================
#ifdef OPT_TERMINAL_MONITOR // turning off terminal monitor will turn these off as well…
#define OPT_SSC_FORWARDER // only useful if terminal monitor is enabled
#define OPT_FIND_SERVO_OFFSETS // Only useful if terminal monitor is enabled
#endif

#define OPT_GPPLAYER

// Which type of control(s) do you want to compile in
#define DBGSerial Serial

#if defined(UBRR1H)
#define SSCSerial Serial1
#else
#endif

#define USEPS2

//==================================================================================================================================
//==================================================================================================================================
//==================================================================================================================================
// CHR-3
//==================================================================================================================================
#define USE_SSC32
//#define cSSC_BINARYMODE 1 // Define if your SSC-32 card supports binary mode.

//[SERIAL CONNECTIONS]

// Warning I will undefine some components as the non-megas don’t have enough memory…
//#undef OPT_FIND_SERVO_OFFSETS

#define cSSC_BAUD 38400 //SSC32 BAUD rate

//--------------------------------------------------------------------
//[Botboarduino Pin Numbers]
#ifdef BOTBOARDUINO
#define SOUND_PIN 5 // Botboarduino JR pin number
#define PS2_DAT 8
#define PS2_CMD 7
#define PS2_SEL 6 // On the PS2 receiver this pin may be called ATT (attention)
#define PS2_CLK 9
// If we are using a SSC-32 then:
// If were are running on an Arduino Mega we will use one of the hardware serial port, default to Serial1 above.
// If on Non mega, if the IO pins are set to 0, we will overload the hardware Serial port
// Else we will user SoftwareSerial to talk to the SSC-32
#define cSSC_OUT 12 //Output pin for Botboard - Input of SSC32 (Yellow)
#define cSSC_IN 13 //Input pin for Botboard - Output of SSC32 (Blue)
#endif

#ifdef BOTBOARD_ARDUINOPROMINI
#define SOUND_PIN 11 // Bot Board JR pin number (with Arduino Pro Mini plugged)
#define PS2_DAT 14
#define PS2_CMD 15
#define PS2_SEL 16
#define PS2_CLK 17
// If we are using a SSC-32 then:
// If were are running on an Arduino Mega we will use one of the hardware serial port, default to Serial1 above.
// If on Non mega, if the IO pins are set to 0, we will overload the hardware Serial port
// Else we will user SoftwareSerial to talk to the SSC-32
#define cSSC_OUT 10 //Output pin for Botboard - Input of SSC32 (Yellow)
#define cSSC_IN 9 //Input pin for Botboard - Output of SSC32 (Blue)
#endif

//====================================================================
//[SSC PIN NUMBERS]
#define cRRCoxaPin 0 //Rear Right leg Hip Horizontal
#define cRRFemurPin 1 //Rear Right leg Hip Vertical
#define cRRTibiaPin 2 //Rear Right leg Knee
//#define cRRTarsPin 3 // Tar

#define cRMCoxaPin 4 //Middle Right leg Hip Horizontal
#define cRMFemurPin 5 //Middle Right leg Hip Vertical
#define cRMTibiaPin 6 //Middle Right leg Knee
//#define cRMTarsPin 7 // Tar

#define cRFCoxaPin 8 //Front Right leg Hip Horizontal
#define cRFFemurPin 9 //Front Right leg Hip Vertical
#define cRFTibiaPin 10 //Front Right leg Knee
//#define cRFTarsPin 11 // Tar

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

#define cLMCoxaPin 20 //Middle Left leg Hip Horizontal
#define cLMFemurPin 21 //Middle Left leg Hip Vertical
#define cLMTibiaPin 22 //Middle Left leg Knee
//#define cLMTarsPin 23 // Tar

#define cLFCoxaPin 24 //Front Left leg Hip Horizontal
#define cLFFemurPin 25 //Front Left leg Hip Vertical
#define cLFTibiaPin 26 //Front Left leg Knee
//#define cLFTarsPin 27 // Tar

//--------------------------------------------------------------------

//-------------------------------------------------------------------- //[MIN/MAX ANGLES]

define cRRCoxaMin1 -650 //Mechanical limits of the Right Rear Leg

define cRRCoxaMax1 650

define cRRFemurMin1 -1050

define cRRFemurMax1 750

define cRRTibiaMin1 -530

define cRRTibiaMax1 900

//#define cRRTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg //#define cRRTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though..

define cRMCoxaMin1 -650 //Mechanical limits of the Right Middle Leg

define cRMCoxaMax1 650

define cRMFemurMin1 -1050

define cRMFemurMax1 750

define cRMTibiaMin1 -530

define cRMTibiaMax1 900

//#define cRMTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg //#define cRMTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though..

define cRFCoxaMin1 -650 //Mechanical limits of the Right Front Leg

define cRFCoxaMax1 650

define cRFFemurMin1 -1050

define cRFFemurMax1 750

define cRFTibiaMin1 -530

define cRFTibiaMax1 900

//#define cRFTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg //#define cRFTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though..

define cLRCoxaMin1 -650 //Mechanical limits of the Left Rear Leg

define cLRCoxaMax1 650

define cLRFemurMin1 -1050

define cLRFemurMax1 750

define cLRTibiaMin1 -530

define cLRTibiaMax1 900

//#define cLRTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg //#define cLRTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though..

define cLMCoxaMin1 -650 //Mechanical limits of the Left Middle Leg

define cLMCoxaMax1 650

define cLMFemurMin1 -1050

define cLMFemurMax1 750

define cLMTibiaMin1 -530

define cLMTibiaMax1 900

//#define cLMTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg //#define cLMTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though..

define cLFCoxaMin1 -650 //Mechanical limits of the Left Front Leg

define cLFCoxaMax1 650

define cLFFemurMin1 -1050

define cLFFemurMax1 750

define cLFTibiaMin1 -530

define cLFTibiaMax1 900

//#define cLFTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg //#define cLFTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though..

//-------------------------------------------------------------------- //[LEG DIMENSIONS] //Universal dimensions for each leg in mm

define cXXCoxaLength 12 // This is for CH3-R with Type 3 legs

define cXXFemurLength 35

define cXXTibiaLength 51

//#define cXXTarsLength 85 // 4DOF only...

define cRRCoxaLength cXXCoxaLength //Right Rear leg

define cRRFemurLength cXXFemurLength

define cRRTibiaLength cXXTibiaLength

//#define cRRTarsLength cXXTarsLength //4DOF ONLY

define cRMCoxaLength cXXCoxaLength //Right middle leg

define cRMFemurLength cXXFemurLength

define cRMTibiaLength cXXTibiaLength

//#define cRMTarsLength cXXTarsLength //4DOF ONLY

define cRFCoxaLength cXXCoxaLength //Rigth front leg

define cRFFemurLength cXXFemurLength

define cRFTibiaLength cXXTibiaLength

//#define cRFTarsLength cXXTarsLength //4DOF ONLY

define cLRCoxaLength cXXCoxaLength //Left Rear leg

define cLRFemurLength cXXFemurLength

define cLRTibiaLength cXXTibiaLength

//#define cLRTarsLength cXXTarsLength //4DOF ONLY

define cLMCoxaLength cXXCoxaLength //Left middle leg

define cLMFemurLength cXXFemurLength

define cLMTibiaLength cXXTibiaLength

//#define cLMTarsLength cXXTarsLength //4DOF ONLY

define cLFCoxaLength cXXCoxaLength //Left front leg

define cLFFemurLength cXXFemurLength

define cLFTibiaLength cXXTibiaLength

//#define cLFTarsLength cXXTarsLength //4DOF ONLY

//--------------------------------------------------------------------

//-------------------------------------------------------------------- //[BODY DIMENSIONS]

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

define cRROffsetX -41 //Distance X from center of the body to the Right Rear coxa

define cRROffsetZ 51 //Distance Z from center of the body to the Right Rear coxa

define cRMOffsetX -51 //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 -41 //Distance X from center of the body to the Right Front coxa

define cRFOffsetZ -51 //Distance Z from center of the body to the Right Front coxa

define cLROffsetX 41 //Distance X from center of the body to the Left Rear coxa

define cLROffsetZ 51 //Distance Z from center of the body to the Left Rearcoxa

define cLMOffsetX 51 //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 41 //Distance X from center of the body to the Left Front coxa

define cLFOffsetZ -51 //Distance Z from center of the body to the Left Front coxa

//-------------------------------------------------------------------- //[START POSITIONS FEET]

define cHexInitXZ 47

define CHexInitXZCos60 24 // COS(60) = .5

define CHexInitXZSin60 41 // sin(60) = .866

define CHexInitY 50 //30

define cRRInitPosX CHexInitXZCos60 //Start positions of the Right Rear leg

define cRRInitPosY CHexInitY

define cRRInitPosZ CHexInitXZSin60

define cRMInitPosX cHexInitXZ //Start positions of the Right Middle leg

define cRMInitPosY CHexInitY

define cRMInitPosZ 0

define cRFInitPosX CHexInitXZCos60 //Start positions of the Right Front leg

define cRFInitPosY CHexInitY

define cRFInitPosZ -CHexInitXZSin60

define cLRInitPosX CHexInitXZCos60 //Start positions of the Left Rear leg

define cLRInitPosY CHexInitY

define cLRInitPosZ CHexInitXZSin60

define cLMInitPosX cHexInitXZ //Start positions of the Left Middle leg

define cLMInitPosY CHexInitY

define cLMInitPosZ 0

define cLFInitPosX CHexInitXZCos60 //Start positions of the Left Front leg

define cLFInitPosY CHexInitY

define cLFInitPosZ -CHexInitXZSin60

//-------------------------------------------------------------------- //[Tars factors used in formula to calc Tarsus angle relative to the ground] //#define cTarsConst 720 //4DOF ONLY //#define cTarsMulti 2 //4DOF ONLY //#define cTarsFactorA 70 //4DOF ONLY //#define cTarsFactorB 60 //4DOF ONLY //#define cTarsFactorC 50 //4DOF ONLY

these are for the phoenix driver i changed the Pwm numbers to match my servos but i feel more changes are needed.

//====================================================================
//Project Lynxmotion Phoenix
//
// Servo Driver - This version is setup to use the SSC-32 to control
// the servos.
//====================================================================
#if ARDUINO>99
#include <Arduino.h> // Arduino 1.0
#else
#include <Wprogram.h> // Arduino 0022
#endif
#include “Hex_Globals.h”
#include “ServoDriver.h”
#ifdef c4DOF
#define NUMSERVOSPERLEG 4
#else
#define NUMSERVOSPERLEG 3
#endif

#ifdef USE_SSC32

//Servo Pin numbers - May be SSC-32 or actual pins on main controller, depending on configuration.
const byte cCoxaPin PROGMEM = {cRRCoxaPin, cRMCoxaPin, cRFCoxaPin, cLRCoxaPin, cLMCoxaPin, cLFCoxaPin};
const byte cFemurPin PROGMEM = {cRRFemurPin, cRMFemurPin, cRFFemurPin, cLRFemurPin, cLMFemurPin, cLFFemurPin};
const byte cTibiaPin PROGMEM = {cRRTibiaPin, cRMTibiaPin, cRFTibiaPin, cLRTibiaPin, cLMTibiaPin, cLFTibiaPin};
#ifdef c4DOF
const byte cTarsPin PROGMEM = {cRRTarsPin, cRMTarsPin, cRFTarsPin, cLRTarsPin, cLMTarsPin, cLFTarsPin};
#endif

// Add support for running on non-mega Arduino boards as well.
#ifdef AVR
#if not defined(UBRR1H)
#if cSSC_IN == 0
#define SSCSerial Serial
#else
SoftwareSerial SSCSerial(cSSC_IN, cSSC_OUT);
#endif
#endif
#endif

//=============================================================================
// Global - Local to this file only…
//=============================================================================

// definition of some helper functions
extern int SSCRead (byte* pb, int cb, word wTimeout, word wEOL);

//--------------------------------------------------------------------
//Init
//--------------------------------------------------------------------
void ServoDriver::Init(void) {
SSCSerial.begin(cSSC_BAUD);

// Lets do the check for GP Enabled here…
#ifdef OPT_GPPLAYER
char abVer[40]; // give a nice large buffer.
byte cbRead;

_fGPEnabled = false; // starts off assuming that it is not enabled…
_fGPActive = false;

#ifdef AVR
#if not defined(UBRR1H)
#if cSSC_IN != 0
SSCSerial.listen();
#endif
#endif
#endif
SSCSerial.print(“ver\r”);
cbRead = SSCRead((byte*)abVer, sizeof(abVer), 10000, 13);

#ifdef DBGSerial
DBGSerial.write(“Check GP Enable: “);
if (cbRead > 0) {
byte iT;
for (iT = 0; iT < cbRead; iT++)
DBGSerial.print(abVer[iT], HEX);
DBGSerial.write((byte*)abVer, cbRead);
}
DBGSerial.print(”\n\r”);
#endif
if ((cbRead > 3) && (abVer[cbRead-3]==‘G’) && (abVer[cbRead-2]==‘P’) && (abVer[cbRead-1]==13))
_fGPEnabled = true; // starts off assuming that it is not enabled…
else
MSound (SOUND_PIN, 2, 40, 2500, 40, 2500);
#endif
}

//--------------------------------------------------------------------
//[GP PLAYER]
//--------------------------------------------------------------------
#ifdef OPT_GPPLAYER

//--------------------------------------------------------------------
//[FIsGPSeqDefined]
//--------------------------------------------------------------------
boolean ServoDriver::FIsGPSeqDefined(uint8_t iSeq)
{
word wGPSeqPtr;

// See if we can see if this sequence is defined
SSCSerial.print(“EER -”);
SSCSerial.print(iSeq2, DEC);
SSCSerial.println(";2");
if ((SSCRead((byte
)&wGPSeqPtr, sizeof(wGPSeqPtr), 1000, 0xffff) == sizeof(wGPSeqPtr)) && (wGPSeqPtr != 0) && (wGPSeqPtr != 0xffff)) {
return true;
}
return false; // nope return error
}

//--------------------------------------------------------------------
// Setup to start sequence number…
//--------------------------------------------------------------------
void ServoDriver::GPStartSeq(uint8_t iSeq)
{
_fGPActive = true;
_iSeq = iSeq;
}

//--------------------------------------------------------------------
//[GP PLAYER]
//--------------------------------------------------------------------
void ServoDriver::GPPlayer(void)
{
byte abStat[4];
byte cbRead;

// BUGBUG:: Should integrate in the newer stuff to all us to control the speed of a sequence.
//Start sequence
if (_fGPActive) {
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess…

SSCSerial.print(“PL0SQ”);
SSCSerial.print(_iSeq, DEC);
SSCSerial.println(“ONCE”); //Start sequence
delay(20);
SSCSerial.flush(); // get rid of anything that was previously queued up…

//Wait for GPPlayer to complete sequence
do {
SSCSerial.print(“QPL0\r”);
cbRead = SSCRead((byte*)abStat, sizeof(abStat), 10000, (word)-1); // [GPStatSeq, GPStatFromStep, GPStatToStep, GPStatTime]
delay(20);
}
while ((cbRead == sizeof(abStat)) && ((abStat[0]!=255) || (abStat[1]!=0) || (abStat[2]!=0) || (abStat[3]!=0)));

g_InputController.AllowControllerInterrupts(true); // Ok to process hserial again…

_fGPActive=false;
}
}
#endif // OPT_GPPLAYER

//------------------------------------------------------------------------------------------
//[BeginServoUpdate] Does whatever preperation that is needed to starrt a move of our servos
//------------------------------------------------------------------------------------------
void ServoDriver::BeginServoUpdate(void) // Start the update
{
}

//====================================================================
//Project Lynxmotion Phoenix
//
// Servo Driver - This version is setup to use the SSC-32 to control
// the servos.
//====================================================================
#if ARDUINO>99
#include <Arduino.h> // Arduino 1.0
#else
#include <Wprogram.h> // Arduino 0022
#endif
#include “Hex_Globals.h”
#include “ServoDriver.h”
#ifdef c4DOF
#define NUMSERVOSPERLEG 4
#else
#define NUMSERVOSPERLEG 3
#endif

#ifdef USE_SSC32

//Servo Pin numbers - May be SSC-32 or actual pins on main controller, depending on configuration.
const byte cCoxaPin PROGMEM = {cRRCoxaPin, cRMCoxaPin, cRFCoxaPin, cLRCoxaPin, cLMCoxaPin, cLFCoxaPin};
const byte cFemurPin PROGMEM = {cRRFemurPin, cRMFemurPin, cRFFemurPin, cLRFemurPin, cLMFemurPin, cLFFemurPin};
const byte cTibiaPin PROGMEM = {cRRTibiaPin, cRMTibiaPin, cRFTibiaPin, cLRTibiaPin, cLMTibiaPin, cLFTibiaPin};
#ifdef c4DOF
const byte cTarsPin PROGMEM = {cRRTarsPin, cRMTarsPin, cRFTarsPin, cLRTarsPin, cLMTarsPin, cLFTarsPin};
#endif

// Add support for running on non-mega Arduino boards as well.
#ifdef AVR
#if not defined(UBRR1H)
#if cSSC_IN == 0
#define SSCSerial Serial
#else
SoftwareSerial SSCSerial(cSSC_IN, cSSC_OUT);
#endif
#endif
#endif

//=============================================================================
// Global - Local to this file only…
//=============================================================================

// definition of some helper functions
extern int SSCRead (byte* pb, int cb, word wTimeout, word wEOL);

//--------------------------------------------------------------------
//Init
//--------------------------------------------------------------------
void ServoDriver::Init(void) {
SSCSerial.begin(cSSC_BAUD);

// Lets do the check for GP Enabled here…
#ifdef OPT_GPPLAYER
char abVer[40]; // give a nice large buffer.
byte cbRead;

_fGPEnabled = false; // starts off assuming that it is not enabled…
_fGPActive = false;

#ifdef AVR
#if not defined(UBRR1H)
#if cSSC_IN != 0
SSCSerial.listen();
#endif
#endif
#endif
SSCSerial.print(“ver\r”);
cbRead = SSCRead((byte*)abVer, sizeof(abVer), 10000, 13);

#ifdef DBGSerial
DBGSerial.write(“Check GP Enable: “);
if (cbRead > 0) {
byte iT;
for (iT = 0; iT < cbRead; iT++)
DBGSerial.print(abVer[iT], HEX);
DBGSerial.write((byte*)abVer, cbRead);
}
DBGSerial.print(”\n\r”);
#endif
if ((cbRead > 3) && (abVer[cbRead-3]==‘G’) && (abVer[cbRead-2]==‘P’) && (abVer[cbRead-1]==13))
_fGPEnabled = true; // starts off assuming that it is not enabled…
else
MSound (SOUND_PIN, 2, 40, 2500, 40, 2500);
#endif
}

//--------------------------------------------------------------------
//[GP PLAYER]
//--------------------------------------------------------------------
#ifdef OPT_GPPLAYER

//--------------------------------------------------------------------
//[FIsGPSeqDefined]
//--------------------------------------------------------------------
boolean ServoDriver::FIsGPSeqDefined(uint8_t iSeq)
{
word wGPSeqPtr;

// See if we can see if this sequence is defined
SSCSerial.print(“EER -”);
SSCSerial.print(iSeq2, DEC);
SSCSerial.println(";2");
if ((SSCRead((byte
)&wGPSeqPtr, sizeof(wGPSeqPtr), 1000, 0xffff) == sizeof(wGPSeqPtr)) && (wGPSeqPtr != 0) && (wGPSeqPtr != 0xffff)) {
return true;
}
return false; // nope return error
}

//--------------------------------------------------------------------
// Setup to start sequence number…
//--------------------------------------------------------------------
void ServoDriver::GPStartSeq(uint8_t iSeq)
{
_fGPActive = true;
_iSeq = iSeq;
}

//--------------------------------------------------------------------
//[GP PLAYER]
//--------------------------------------------------------------------
void ServoDriver::GPPlayer(void)
{
byte abStat[4];
byte cbRead;

// BUGBUG:: Should integrate in the newer stuff to all us to control the speed of a sequence.
//Start sequence
if (_fGPActive) {
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess…

SSCSerial.print(“PL0SQ”);
SSCSerial.print(_iSeq, DEC);
SSCSerial.println(“ONCE”); //Start sequence
delay(20);
SSCSerial.flush(); // get rid of anything that was previously queued up…

//Wait for GPPlayer to complete sequence
do {
SSCSerial.print(“QPL0\r”);
cbRead = SSCRead((byte*)abStat, sizeof(abStat), 10000, (word)-1); // [GPStatSeq, GPStatFromStep, GPStatToStep, GPStatTime]
delay(20);
}
while ((cbRead == sizeof(abStat)) && ((abStat[0]!=255) || (abStat[1]!=0) || (abStat[2]!=0) || (abStat[3]!=0)));

g_InputController.AllowControllerInterrupts(true); // Ok to process hserial again…

_fGPActive=false;
}
}
#endif // OPT_GPPLAYER

//------------------------------------------------------------------------------------------

Double of above.

//--------------------------------------------------------------------
//[FREE SERVOS] Frees all the servos
//--------------------------------------------------------------------
void ServoDriver::FreeServos(void)
{
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess…
for (byte LegIndex = 0; LegIndex < 32; LegIndex++) {
SSCSerial.print("#");
SSCSerial.print(LegIndex, DEC);
SSCSerial.print(“P0”);
}
SSCSerial.print(“T200\r”);
g_InputController.AllowControllerInterrupts(true);
}

//==============================================================================
// SSC Forwarder - used to allow things like Lynxterm to talk to the SSC-32
// through the Arduino… Will see if it is fast enough…
//==============================================================================
#ifdef OPT_SSC_FORWARDER
void ServoDriver::SSCForwarder(void)
{
MSound(SOUND_PIN, 1, 1000, 2000); //sound SOUND_PIN, [50\4000]
delay(2000);
int sChar;
int sPrevChar;
DBGSerial.println(“SSC Forwarder mode - Enter $ to exit”);

while(digitalRead(PS2_CMD)) {
if ((sChar = DBGSerial.read()) != -1) {
SSCSerial.write(sChar & 0xff);
if (((sChar == ‘\n’) || (sChar == ‘\r’)) && (sPrevChar == ‘$’))
break; // exit out of the loop
sPrevChar = sChar;
}

if ((sChar = SSCSerial.read()) != -1) {
DBGSerial.write(sChar & 0xff);
}
}
DBGSerial.println(“Exited SSC Forwarder mode”);
}
#endif // OPT_SSC_FORWARDER

//==============================================================================
// Quick and dirty helper function to read so many bytes in from the SSC with a timeout and an end of character marker…
//==============================================================================
int SSCRead (byte* pb, int cb, word wTimeout, word wEOL)
{
int ich;
byte* pbIn = pb;
unsigned long ulTimeLastChar = micros();
while (cb) {
while (!SSCSerial.available()) {
// check for timeout
if ((word)(micros()-ulTimeLastChar) > wTimeout) {
return (int)(pb-pbIn);
}
}
ich = SSCSerial.read();
*pb++ = (byte)ich;
cb–;

if ((word)ich == wEOL)
break; // we matched so get out of here.
ulTimeLastChar = micros(); // update to say we received something
}

return (int)(pb-pbIn);
}

//==============================================================================
// FindServoOffsets - Find the zero points for each of our servos…
// Will use the new servo function to set the actual pwm rate and see
// how well that works…
//==============================================================================
#ifdef OPT_FIND_SERVO_OFFSETS

void ServoDriver::FindServoOffsets()
{
// not clean but…
byte abSSCServoNum[NUMSERVOSPERLEG6]; // array of servos…
signed char asOffsets[NUMSERVOSPERLEG
6]; // we have 18 servos to find/set offsets for…
signed char asOffsetsRead[NUMSERVOSPERLEG*6]; // array for our read in servos…

static char *apszLegs = {“RR”,“RM”,“RF”, “LR”, “LM”, “LF”}; // Leg Order
static char *apszLJoints = {" Coxa", " Femur", " Tibia", " tArs"}; // which joint on the leg…

byte szTemp[5];
byte cbRead;

int data;
short sSN ; // which servo number
boolean fNew = true; // is this a new servo to work with?
boolean fExit = false; // when to exit
int ich;

if (CheckVoltage()) {
// Voltage is low…
Serial.println(“Low Voltage: fix or hit $ to abort”);
while (CheckVoltage()) {
if (Serial.read() == ‘$’) return;
}
}

// Fill in array of SSC-32 servo numbers
for (sSN=0; sSN < 6; sSN++) { // Make sure all of our servos initialize to 0 offset from saved.
abSSCServoNum[sSNNUMSERVOSPERLEG + 0] = pgm_read_byte(&cCoxaPin[sSN]);
abSSCServoNum[sSN
NUMSERVOSPERLEG + 1] = pgm_read_byte(&cFemurPin[sSN]);
abSSCServoNum[sSNNUMSERVOSPERLEG + 2] = pgm_read_byte(&cTibiaPin[sSN]);
#ifdef c4DOF
abSSCServoNum[sSN
NUMSERVOSPERLEG + 3] = pgm_read_byte(&cTarsPin[sSN]);
#endif
}
// now lets loop through and get information and set servos to 1500
for (sSN=0; sSN < 6*NUMSERVOSPERLEG; sSN++ ) {
asOffsets[sSN] = 0;
asOffsetsRead[sSN] = 0;

SSCSerial.print(“R”);
SSCSerial.println(32+abSSCServoNum[sSN], DEC);
// now read in the current value… Maybe should use atoi…
cbRead = SSCRead((byte*)szTemp, sizeof(szTemp), 10000, 13);
if (cbRead > 0)
asOffsetsRead[sSN] = atoi((const char *)szTemp);

SSCSerial.print("#");
SSCSerial.print(abSSCServoNum[sSN], DEC);
SSCSerial.println(“P1500”);
}

// OK lets move all of the servos to their zero point.
Serial.println(“Find Servo Zeros.\n$-Exit, ± changes, *-change servo”);
Serial.println(" 0-5 Chooses a leg, C-Coxa, F-Femur, T-Tibia");

sSN = true;
while(!fExit) {
if (fNew) {
Serial.print(“Servo: “);
Serial.print(apszLegs[sSN/NUMSERVOSPERLEG]);
Serial.print(apszLJoints[sSN%NUMSERVOSPERLEG]);
Serial.print(”(”);
Serial.print(asOffsetsRead[sSN]+asOffsets[sSN], DEC);
Serial.println(")");

// Now lets wiggle the servo
SSCSerial.print("#");
SSCSerial.print(abSSCServoNum[sSN], DEC);
SSCSerial.print(“P”);
SSCSerial.print(1500+asOffsets[sSN]+250, DEC);
SSCSerial.println(“T250”);
delay(250);

SSCSerial.print("#");
SSCSerial.print(abSSCServoNum[sSN], DEC);
SSCSerial.print(“P”);
SSCSerial.print(1500+asOffsets[sSN]-250, DEC);
SSCSerial.println(“T500”);
delay(500);

SSCSerial.print("#");
SSCSerial.print(abSSCServoNum[sSN], DEC);
SSCSerial.print(“P”);
SSCSerial.print(1500+asOffsets[sSN], DEC);
SSCSerial.println(“T250”);
delay(250);

fNew = false;
}

//get user entered data
data = Serial.read();
//if data received
if (data !=-1) {
if (data == ‘$’)
fExit = true; // not sure how the keypad will map so give NL, CR, LF… all implies exit

else if ((data == ‘+’) || (data == ‘-’)) {
if (data == ‘+’)
asOffsets[sSN] += 5; // increment by 5us
else
asOffsets[sSN] -= 5; // increment by 5us

Serial.print(" ");
Serial.println(asOffsetsRead[sSN]+asOffsets[sSN], DEC);

SSCSerial.print("#");
SSCSerial.print(abSSCServoNum[sSN], DEC);
SSCSerial.print(“P”);
SSCSerial.print(1500+asOffsets[sSN], DEC);
SSCSerial.println(“T100”);
} else if ((data >= ‘0’) && (data <= ‘5’)) {
// direct enter of which servo to change
fNew = true;
sSN = (sSN % NUMSERVOSPERLEG) + (data - ‘0’)NUMSERVOSPERLEG;
} else if ((data == ‘c’) && (data == ‘C’)) {
fNew = true;
sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 0;
} else if ((data == ‘c’) && (data == ‘C’)) {
fNew = true;
sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 1;
} else if ((data == ‘c’) && (data == ‘C’)) {
// direct enter of which servo to change
fNew = true;
sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 2;
} else if (data == '
’) {
// direct enter of which servo to change
fNew = true;
sSN++;
if (sSN == 6NUMSERVOSPERLEG)
sSN = 0;
}
}
}
Serial.print("Find Servo exit ");
for (sSN=0; sSN < 6
NUMSERVOSPERLEG; sSN++){
Serial.print(“Servo: “);
Serial.print(apszLegs[sSN/NUMSERVOSPERLEG]);
Serial.print(apszLJoints[sSN%NUMSERVOSPERLEG]);
Serial.print(”(”);
Serial.print(asOffsetsRead[sSN]+asOffsets[sSN], DEC);
Serial.println(")");
}

Serial.print("\nSave Changes? Y/N: ");

//get user entered data
while (((data = Serial.read()) == -1) || ((data >= 10) && (data <= 15)))
;

if ((data == ‘Y’) || (data == ‘y’)) {
// Ok they asked for the data to be saved. We will store the data with a
// number of servos (byte)at the start, followed by a byte for a checksum…followed by our offsets array…
// Currently we store these values starting at EEPROM address 0. May later change…
//

for (sSN=0; sSN < 6*NUMSERVOSPERLEG; sSN++ ) {
SSCSerial.print(“R”);
SSCSerial.print(32+abSSCServoNum[sSN], DEC);
SSCSerial.print("=");
SSCSerial.println(asOffsetsRead[sSN]+asOffsets[sSN], DEC);
delay(10);
}

// Then I need to have the SSC-32 reboot in order to use the new values.
delay(10); // give it some time to write stuff out.
SSCSerial.println(“GOBOOT”);
delay(5); // Give it a little time
SSCSerial.println(“g0000”); // tell it that we are done in the boot section so go run the normall SSC stuff…
delay(500); // Give it some time to boot up…

} else {
void LoadServosConfig();
}

FreeServos();

}
#endif // OPT_FIND_SERVO_OFFSETS

#endif

the config files and the phoenix_drver are the only files i have modified.

any help would be appreciated

Looks like its moving each servo more than it needs to. The front legs come all the way out to the most forward position. Same with the back. Middle looks fine.

Also when it brings the femurs down they are coming down further than the ground level. Or at least it tries.

MorganS: We can't debug snippets. 90% of the time that somebody posts a snippet of code that they think has a problem, it turns out that the problem is elsewhere. C coding by beginners has simple mistakes in the declaration of variables, because the beginners don't think it's important, which is why they didn't post the declaration part of their code. QED.

I dont understand your statement really. Are we not on the arduino programming thread? Is that not what we do here? Im pretty sure we do help people with thier code since, well look at all the threads.

The problem is there is no list of parameters that need to be changed and because of that i need help figuring out what part of my code needs to be changed.

If you do have a list of the variables that need to be changed please post it.

Other wise i have no choice but to post code and ask if its right.

If you do not want to help you do not have to, but im hoping there is someone here with the experience to help me figure out what does or doesnt need to be changed.

Thank you ahead of time to those here to help.