Refactoring classes in Arduino Phoenix code

kurte · June 4, 2012, 8:54pm

Sounds good.

I found the code I mentioned It was in an older version of the code that I modified to use Zenta’s version of the DIY with the extra knobs… If you were in walk mode and hit the 9 key it would update the positions of the legs. The code below is specific to round hexs… As I mentioned earlier, Kåre has far more eloquent code as demonstrated in MorpHex…

[code] elseif iNumButton = 9
’ We wish to play with the leg positions… to see if I can make them more up and down…
'The Y from Pep is computed normally something like:
’ Y = FemurLengthCos(90-FemurAngle)
’ + Tibia Lengthcos((90-FemurAngle)+(TibiaAngle - 90))
'Note: the 90s cancel out if we want the foot flat we know the whole Tibialength will go into Y
’ so we can compute the needed femur angle
’ FemurAngle =90 - ACOS(( Y-TibiaLength)/FemurLength)
'From that we compute TibiaAngle = -90+FemurAngle

				GOSUB GetArcCos ((CHexInitY+BodyPosY-cTibiaLength) * c4DEC)/cFemurLength ] ' not sure yet of the appropriate scale here...
				FemurAngle1H = 900 - AngleRad4*180/3141		; Convert to 1/10 degrees
				TibiaAngle1H = FemurAngle1H - 900;
				' now legs compute XZ value for how far the leg will be from the body given the above angles...
				' should be: CoxaLen + sin(90-FemurAngle)*FemurLength
				gosub GetSinCos[900 - FemurAngle1H]
				XZLength1 = cCoxaLength + (sin4*cFemurLength )/c4dec			; get our length from body...
				' now print this out and see if it looks right..
						
				; now lets see what happens when we change the leg positions...
				for LegIndex  = 0 to 5
					gosub GetSinCos[cCoxaAngle1(LegIndex)]
					LegPosX(LegIndex) = (cos4 * XZLength1)/c4dec	;Set start positions for each leg
					LegPosZ(LegIndex) = -(sin4 * XZLength1)/c4dec

#ifdef HSP_DEBUG
hserout HSP_DEBUG, “(”, sdec LegPosX(LegIndex), “,”, sdec LegPosZ(LegIndex),") “]
#else
serout s_out, i9600, “(”, sdec LegPosX(LegIndex), “,”, sdec LegPosZ(LegIndex),”) "]
#endif
next
#ifdef HSP_DEBUG
hserout HSP_DEBUG, [13]
#else
serout s_out, i9600, [13]
#endif
bPacket(PKT_RJOYUD) = 127-cTravelDeadZone ; Hack see if we can make it walk 1 step to get to new positions…
ENDIF
[/code]
So may try playing around. Again not sure if it should be automatically done when the robot rises and lowers or if there should be some input to do so…

Kurt

Edit: I see Zenta gave some more info here that I will look at… Thanks Zenta

zenta · June 4, 2012, 9:51pm

Hi Kurt,

Did this code work?

The LegPos values can’t be changed unless you lift the leg first. In my MorpHex code I modified many sections of the code like the gait and LegIK subs + much more.

This is a small portion of the code from the controller file:

;This code make it possible to do bodymorphing combined with leglifting (no actual walking), just like we do for SphereLevel 1 IF (GearAngle1 =(400+ ((bPacket(PKT_RPOT)*1400)/255))) THEN IF (CycleCounter<= (StepsInGait-1)) THEN ;Make sure that one tripod gait (8 steps) are completed TravelRequest = 1 CycleCounter = CycleCounter + 1 ELSE TravelRequest = 0 ;Stop gait when one complete gait cycle is done ENDIF ELSE TravelRequest = 1 CycleCounter = 0 ;Reset counter ENDIF

What this code do is that it forces the gait section to lift the legs without walking in any direction. In the “Leg middle up position” (in the gait sub) I update the new Init positions (actually I’m using another variable though). Very shortly explained.

The question is when do we want to move from one init position to the other? Like you mentioned, maybe this should be done automatically. Perhaps a one button function for lowering the body to ground; first update the initpositions, then move the body down? And vice versa when moving from lowered state to walking state.

kurte · June 4, 2012, 10:51pm

My Hack at the end of this:

bPacket(PKT_RJOYUD) = 127-cTravelDeadZone ; Hack see if we can make it walk 1 step to get to new positions...

Made it walk one step at the minimum distance which caused all the legs to lift once and move into their new locations. Was not eloquent, but sort-of got the job done, with a minumum of change…

As you say, when do you do this? If it is only to allow the hex to move down the extra 30/40 mm to the ground, I am not sure if it is worth the extra code and effort. If it is more a dynamic thing that for example allows the legs to adjust at different heights for better movement or object detection than it is for sure probably worth it. I may play around with some of this. Would be great to see what all you did to make it work earlier…

Kurt

kurte · June 5, 2012, 3:43am

I see what you were doing with your code with setting TravelRequest. However how did you keep the GaitSeq function from overwriting that value? I was instead thinking of adding a new variable to my Control Input structure, that maybe is something like fForceStepSeq which gets ored into TravelRequest. Alternatively could be a counter that if > 0 forces it and the GaitSeq could decrement it each time through (if not zero…). Right now I am hacking it on the L3 button of the PS2… Will try it out tomorrow…

Kurt

kurte · June 5, 2012, 1:44pm

I have been playing with this some last night and this morning…
I added this function:

[code]#define DEBUG
void AdjustLegPositionsToBodyHeight(void)
{
// We wish to play with the leg positions… to see if I can make them more up and down…
//The Y from Pep is computed normally something like:
// Y = FemurLengthCos(90-FemurAngle)
// + Tibia Lengthcos((90-FemurAngle)+(TibiaAngle - 90))
//Note: the 90s cancel out if we want the foot flat we know the whole Tibialength will go into Y
// so we can compute the needed femur angle
// FemurAngle =90 - ACOS(( Y-TibiaLength)/FemurLength)
//From that we compute TibiaAngle = -90+FemurAngle

 AngleRad4 = GetArcCos(((long)((CHexInitY+g_InControlState.BodyPos.y-cXXTibiaLength) * c4DEC))/cXXFemurLength);
 short FemurAngle1H = 900 - ((long)((long)AngleRad4)*180)/3141;      // Convert to 1/10 degrees
 short TibiaAngle1H = FemurAngle1H - 900;
 // now legs compute XZ value for how far the leg will be from the body given the above angles...
 // should be: CoxaLen + sin(90-FemurAngle)*FemurLength
 GetSinCos(900 - FemurAngle1H);
 short XZLength1 = cXXCoxaLength + ((long)(sin4*cXXFemurLength ))/c4DEC;         // get our length from body...
 // now print this out and see if it looks right..

#ifdef DEBUG
if (g_fDebugOutput) {
Serial.println(g_InControlState.BodyPos.y, DEC);
Serial.println(((long)((CHexInitY+g_InControlState.BodyPos.y-cXXTibiaLength) * c4DEC))/cXXFemurLength, DEC);
Serial.println(AngleRad4, DEC);
Serial.println(FemurAngle1H, DEC);
Serial.println(TibiaAngle1H, DEC);
Serial.println(XZLength1, DEC);
}
#endif
//now lets see what happens when we change the leg positions…
for (uint8_t LegIndex = 0; LegIndex <= 5; LegIndex++) {
#ifdef DEBUG
if (g_fDebugOutput) {
DBGSerial.print("(");
DBGSerial.print(LegPosX[LegIndex], DEC);
DBGSerial.print(",");
DBGSerial.print(LegPosZ[LegIndex], DEC);
DBGSerial.print(")->");
}
#endif
GetSinCos((short)pgm_read_word(&cCoxaAngle1[LegIndex]));
LegPosX[LegIndex] = ((long)((long)cos4 * XZLength1))/c4DEC; //Set start positions for each leg
LegPosZ[LegIndex] = -((long)((long)sin4 * XZLength1))/c4DEC;
#ifdef DEBUG
if (g_fDebugOutput) {
DBGSerial.print("(");
DBGSerial.print(LegPosX[LegIndex], DEC);
DBGSerial.print(",");
DBGSerial.print(LegPosZ[LegIndex], DEC);
DBGSerial.print(") “);
}
#endif
}
#ifdef DEBUG
if (g_fDebugOutput) {
DBGSerial.println(”");
}
#endif
// Make sure we cycle through one gait to have the legs all move into their new locations…
g_InControlState.ForceGaitStepCnt = StepsInGait;

}[/code]
To the end of the main file. In the PS2 code I added a call to this if you press L3 in walk mode. I added the new variable g_InControlState.ForceGaitStepCnt, that in:
GaitSeq, I used like:

[code]//[GAIT Sequence]
void GaitSeq(void)
{
//Check if the Gait is in motion
TravelRequest = (abs(g_InControlState.TravelLength.x)>cTravelDeadZone) || (abs(g_InControlState.TravelLength.z)>cTravelDeadZone)
|| (abs(g_InControlState.TravelLength.y)>cTravelDeadZone) || (g_InControlState.ForceGaitStepCnt != 0);
if (NrLiftedPos == 5)
LiftDivFactor = 4;
else
LiftDivFactor = 2;

//Calculate Gait sequence
LastLeg = 0;
for (LegIndex = 0; LegIndex <= 5; LegIndex++) { // for all legs
if (LegIndex == 5) // last leg
LastLeg = 1 ;

    Gait(LegIndex);
}    // next leg

// If we have a force count decrement it now... 
if (g_InControlState.ForceGaitStepCnt)
  g_InControlState.ForceGaitStepCnt--;

}
[/code]
But I don’t think this will get what I want . That is this was working nicely with the ?H3-R with the contact switches as the Tibia length was small and it made sense to make the leg go straight up and down… But in this case that does not work… Could change instead to maybe have a set of values, like:
When Height < Y make Leg length XZ1, when < Y2 make it XZ2, …
Suggestions?

Kurt

dev5994 · June 5, 2012, 3:05pm

Please keep in mind guys that we’re wanting to use github for code development so if you make permanent changes just commit the changes to the Lynxmotion github 3DOF-4DOF-Hex repo