Very Complicated Debugging of a 12 Servo Biped

Using Arduino Programming Questions
1

Hi forum, once again I need help and advice for my Biped project years in the making.

After hitting a brick wall in programming, I picked up my robot recently, 3 or 4 months from setting it down and started coding again.
I fixed some hardware problems and looked at the code. After spending some time on it, so tired of doing calculations, I just started typing until I got something that "just works".
Now I've run into a problem. After setting up what someone calls "asynchronous movement", the legs seem to work correctly, but I need them to shift the weight of the biped when stepping.

The servos that control weight shifting are servos 1(R hip) and 6(R Ankle) as well as 17(L hip) and 22(L Ankle)

If anyone can actually read the written code, I need to know where to put these in the loop and how to properly execute them. The feet need to stay flat to the floor, in other words, if the hip moves 90 degrees CC, the ankle has to move 90 degrees C. The feet also have to come a fair bit off the ground to allow proper movement, but also have to flatten out to prepare for landing and weight shift in the opposite direction.

The code unfortunately far exceeds the character limit, so I'm attaching an .ino file unless someone sees a better way for me to post...

Any insight would extremely help, and I greatly commend anyone with the patience to read through this entirely

-Quinn

_5_11_13Scout.ino (9.01 KB)

2

I haven't looked at your code but I did once see a fascinating animation of human walking and it was suddenly very clear that the weight shifting comes first and we only move our feet to stop from falling over.

...R

3

I know how to shift weight, just not when and where. Really, I guess my problem is that I'm going to cry if I have to look at my code one more time

4

That sounds like a complex project and pretty hard to diagnose most problems without being able to see it running, but in terms of calculating joint angles and so on you might think about deciding what your desired position, orientation and speed is for each foot and then use inverse kinematics to calculate the joint positions and speeds needed to achieve that.

5

The code attached to the first post does exactly that, it runs through a custom IK algorithm. What I need to know how to do is how to move servos while a for loop is running. It can't interrupt the movement, but it should have the ability to interrupt movement if I need it to.

Move Leg
Move Leg
Move Leg and shift servos
Move Leg

OR

Move Leg
Move Leg
Move Leg
Halt Movement to safely allow movement change
Move Leg

6

I have built a couple of hexapods, so have some experience controlling lots of servos. I would suggest you don't use a for loop and totally forget about the delay function. I just use the millis function. It is not that hard, just take a look at the blink without delay example in the IDE.

7

I have now had a brief look at your code but I can't see where the servos are declared or attached, OR, more importantly where you do servo.write().

So the following comments are made in that vacuum and may be irrelevant.

I would update the servo positions regularly using an interrupt to run the update loop. The code in the interrupt service routine (ISR) should be as short as possible.

I would store the values to be sent to the servos in one or more arrays from which the ISR could easily pick the correct value.

I would then use my loop() to figure out the logic of what moves are needed and to change the values in the array(s) as appropriate so that when the ISR is triggered it uses the new value(s).

This will completely separate the timing of the servo movements from the code that has to do the "thinking". It might also allow the thinking code to be on a PC which just sends instructions to the device.

...R

8

Thanks for the replies, servos are initiated and controlled in the buildmove and finishmove subs. These are the commands that the SSC-32 receives to write servo positions.

I have seriously given my best effort to ditch the loop, but it seems impossible, this is what happens:

An arc is calculated, X and Y coordinates are chosen and then looped from beginning to end. Each leg is assigned the values given off through the numerous algorithms needed to find the servo locations of the coordinates. This arc is the basic stepping motion. The legs are then set to be .5 of a fraction off. One leg moves, then when it is halfway done with a full cycle (two steps) the other leg starts, step step step step etc.

I will cry if I have to rewrite the code a fourth time as I have spent a good part of two years on this project. Albeit the code may be wrong, but i just want to see it work a tiny bit, however awful the code is, before I rewrite it again.

I don't quite understand how I would use the millis function still. I have to use loop to find the values along an arc, I have to loop the commands to the SSC-32 to continue the stepping motion. It's not as easy as going "hey robot take a step forward in 30 seconds"

Anyway, thanks guys for the help

9

Another note, maybe it's not clear how I'm going about coding this. I'm not doing a stop motion animation kind of code, think ARCher, the other robot similar to mine. Now they are built using the same sort of parts, but different styles. ARCher and my yet unnamed project both run off of custom coded Inverse Kinematic equations.

The heart of my code is the IK subroutine. X and Y are incremented according to a pre-calculated arc, and the IK subroutine calculates every single servo position and writes the servos

10

SilentDemon555:
What I need to know how to do is how to move servos while a for loop is running. It can't interrupt the movement, but it should have the ability to interrupt movement if I need it to.

You need to have some algorithm for determining the required position and speed of each servo. That might be a hard-coded timed sequence of actions if the biped is inherently stable when walking, or it might be via a feedback loop monitoring its state of balance.

For timed movements, you have a piece of code that runs repeatedly for each servo which calculates the required position for that servo at that time, and if it is not the same as the current position it moves the servo to the required position. You simply call this repeatedly for all servos fast enough to produce movements as smooth as you require.

I imagine that the code to calculate the required position and speed will also be controlled by some code that controls the gait i.e. decides when the left leg needs to swing forward, when it needs to stop, when it needs to straighten the leg to contact the ground, and so on. This gait management code would also run repeatedly and determine where the biped was in the walking cycle and whether it was necessary to advance to the next part of the walking cycle. (This is also where I'd expect to see weight transfer initiated.)

11

Once again, servo timing and speed are calculated by the SSC-32, the servo positions are handed out in the IK algorithm
Okay this is what I implemented:

void Shift(float frac){
int stepcounter;
int rollfootpulse;

Serial.print(frac);

Serial.print(stepcounter);
 
  if(frac == 0){
   rollfootpulse = D2Pminus(1500, 8); //D2P  calculates the degree needed to move, in this case 8 degrees
          buildmove(6, rollfootpulse);
          buildmove(22, rollfootpulse);
          buildmove(2, -rollfootpulse);
          buildmove(18, -rollfootpulse);
          finishmove(500);
          delay(1000);
         Serial.println("1");
          
  }
  if(frac == .5){
          buildmove(6, servo6);
          buildmove(22, servo22);
          buildmove(2, servo2);
          buildmove(18, servo18);
          finishmove(500);  
         delay(1000); 
         Serial.println("2");
  }
   if(frac == .6){
   rollfootpulse = D2Pplus(1500, 14);
          buildmove(6, rollfootpulse);
          buildmove(22, rollfootpulse);
          buildmove(2, -rollfootpulse);
          buildmove(18, -rollfootpulse);
          finishmove(500);
          delay(1000);
          Serial.println("3");
         
          
  }
  if(frac == .9){
          buildmove(6, servo6);
          buildmove(22, servo22);
          buildmove(2, servo2);
          buildmove(18, servo18);
          finishmove(500); 
          delay(1000);
          Serial.println("4");
  }
}

Each .5 of Fraction done is a full step. 0 ----.5 = 1 full left step, .5----1= 1 full right step
The code is extremely rough, but seems to work... sometimes.
I can't call 1.0 because for some reason, even though frac can be <= 1.0, it only reaches .9 at greatest before relooping.

Is there a cleaner way to add this, it is called every time Fractiondone is increased, once per loop

12

Does your sketch send the servo positions using Serial.print statements? (I had assumed they were just for debugging purposes.)

If so perhaps you can explain what information you need to send with Serial.print and how it is dealt with by whatever receives it? (I can't imagine that Serial.print is fast enough to manage every single minor servo move.)

I imagine (as I hinted in my earlier post) that the centre-of-gravity shift must be an integral part of a leg movement, and not something that is "bolted on" afterwards. In other words as the right leg lifts off the floor the left leg tilts forward while the left foot remains flat on the floor. If you can imagine (at a particular moment) the left leg being at an angle of 2 degrees to the vertical perhaps the included angle between the left and right legs should be 4 deg at the same moment.

Just my uneducated 3 cents ...

...R

13

SilentDemon555:
Is there a cleaner way to add this, it is called every time Fractiondone is increased, once per loop

At the moment your code is all blocking. Given the complexity of the movements you're trying to create, I think a non-blocking structure would work better.

Instead of:

for (FractionDone = 0; FractionDone <= 1; FractionDone = FractionDone + FracStep)
{
   // move servos ...
   delay(1000);
}

use the approach demonstrated in the 'blink without delay sketch to determine whether it's time to move either leg, and then move it.

This would make it far easier for you to control the speed and timing of each leg movement.

You're asking specifically about how to incorporate a weight shift into the walking sequence. It's not clear from the code what mechanism you use to control weight shifting. Do you control it via hips or ankles, or just by controlling the length of each leg and letting the grounded foot roll sideways? If it's initiated explicitly by moving hips/ankles then you'd need to have a sequence of movements sycnronised with the leg movement. If it's done by having the whole biped rock sideways then I guess you need to recalculate the arc that each foot moves through while it's on the ground so that the effective length of the leg varies cyclically during the stride so that the leg effectively shortens when you want the biped to fall towards that side, and lengthens when you want to push it away. The resulting sideways movement would cause weight transfer.