In the previous post, I went over my initial linear approach to stepping and why it didn’t work out so well. This post outlines my current method which is working much better.
The new method uses a sinusoidal “arch” shaped step in the ZX plane. This also works out well since a single sine function can cover the raise and lower part of the step in one time segment. By using a sinusoidal function for the front-to-back stepping in the X direction as well, i could cut the number of case in half to just two. The final benefit is that it produces a very smooth and natural motion with the smooth curves and foot speeds. Here’s what step code looks like:
switch (caseStep[legNum]){
case 1: //forward raise and lower
leg[legNum].footPos.x = -amplitudeX*cos(M_PI*tick/numTicks);
leg[legNum].footPos.y = -amplitudeY*cos(M_PI*tick/numTicks);
leg[legNum].footPos.z = -abs(amplitudeZ)*sin(M_PI*tick/numTicks);
if( tick >= numTicks-1 ) caseStep[legNum] = 2;
break;
case 2: // pull back
leg[legNum].footPos.x = amplitudeX*cos(M_PI*tick/numTicks);
leg[legNum].footPos.y = amplitudeY*cos(M_PI*tick/numTicks);
leg[legNum].footPos.z = 0;
if( tick >= numTicks-1 ) caseStep[legNum] = 1;
break;
}// end of case statement
As you noticed, the sequencing itself is carried out by the case statement block. They key to the sequencing is in the caseStep[] array. The array has six elements, one for each leg. The variable defines at which case each leg begins. For a tripod gait, the front right leg (#1), rear right (#3), and middle left (#5) leg all start at the first case and step together. The other legs start 180deg off which is the second case. So for my prior four-case linear “triangle” step function, caseStep[] is defined as {1,3,1,3,1,3}. For the new two-case “sine” step function, caseStep[] is defined as {1,2,1,2,1,2}.
Each case statement increments caseStep[] to the next case in order to advance to the next portion of the foot step. The final code looks like:
/*************************************************
tripodGaitSine()
**************************************************/
void tripodGaitSine(){
float sinRotZ, cosRotZ;
int totalX, totalY;
float rotSpeedOffsetX[6], rotSpeedOffsetY[6];
float amplitudeX, amplitudeY, amplitudeZ;
int duration;
int numTicks;
int speedX, speedY, speedR;
if( (abs(commanderInput.Xspeed) > 5) || (abs(commanderInput.Yspeed) > 5) || (abs(commanderInput.Rspeed) > 5 ) ){
duration = 500; //duration of one step cycle (ms)
numTicks = round(duration / SERVO_UPDATE_PERIOD / 2.0); //total ticks divided into the two cases
speedX = 180*commanderInput.Xspeed/127; //180mm/s top speed for 180mmm stride in one sec
speedY = 180*commanderInput.Yspeed/127; //180mm/s top speed
speedR = 40*commanderInput.Rspeed/127; //40deg/s top rotation speed
sinRotZ = sin(radians(speedR));
cosRotZ = cos(radians(speedR));
for( int legNum=0; legNum<6; legNum++){ totalX = leg[legNum].initialFootPos.x + leg[legNum].legBasePos.x; totalY = leg[legNum].initialFootPos.y + leg[legNum].legBasePos.y; rotSpeedOffsetX[legNum] = totalY*sinRotZ + totalX*cosRotZ - totalX; rotSpeedOffsetY[legNum] = totalY*cosRotZ - totalX*sinRotZ - totalY; if( abs(speedX + rotSpeedOffsetX[legNum]) > abs(speedY + rotSpeedOffsetY[legNum]) )
amplitudeZ = ((speedX + rotSpeedOffsetX[legNum])*duration/3000.0);
else
amplitudeZ = ((speedY + rotSpeedOffsetY[legNum])*duration/3000.0);
amplitudeX = ((speedX + rotSpeedOffsetX[legNum])*duration/2000.0);
amplitudeY = ((speedY + rotSpeedOffsetY[legNum])*duration/2000.0);
switch (caseStep[legNum]){
case 1: //forward raise and lower
leg[legNum].footPos.x = -amplitudeX*cos(M_PI*tick/numTicks);
leg[legNum].footPos.y = -amplitudeY*cos(M_PI*tick/numTicks);
leg[legNum].footPos.z = -abs(amplitudeZ)*sin(M_PI*tick/numTicks);
if( tick >= numTicks-1 ) caseStep[legNum] = 2;
break;
case 2: // pull back
leg[legNum].footPos.x = amplitudeX*cos(M_PI*tick/numTicks);
leg[legNum].footPos.y = amplitudeY*cos(M_PI*tick/numTicks);
leg[legNum].footPos.z = 0;
if( tick >= numTicks-1 ) caseStep[legNum] = 1;
break;
}// end of case statement
}// end of loop over legs
if (tick < numTicks-1) tick++;
else tick = 0;
}//end if joystick active
}
I think it needs a little fine tuning of the speeds and step durations, but its essentially done. B.E.T.H. is now fully mobile!
Check out where this all began.
