First the easy part: enum…
The enum is a quick way to do defines, where basically the next element in the list is equal to the previous one plus 1…
so: enum {SERVO_RH=0, SERVO_RK, SERVO_RA, SERVO_LH, SERVO_LK, SERVO_LA};
is like:
#define SERVO_RH 0
#define SERVO_RK 1
...
#define SERVO_LA 5
The next difference in my version is I converted your individual defined servos to be an array of servos.
Servo aServos[6];
Now to the harder stuff…
Most Servos require a pulse about 50 times per second. The command servo.write (or servo.writeMicroseconds), sets the width of each of these pulses. There is lots of details about the pulse requirements up on the net such as: viewtopic.php?f=31&t=3172
So for example if in your code you wish for a servo to move from 90 degrees to 120 degrees in a half second (500ms), if you did something like:
MyServo.write(90);
delay(500);
MyServo.write(120);
That after waiting for a half second the servo would jump to 120 as fast as the servo could get there, which makes it very jerky. So instead suppose you did something like:
MyServo.write(90);
delay(20);
MyServo.write(91);
delay(20);
MyServo.write(92);
delay(20);
...
MyServo.write(120);
The servo would only move small increments at a time and as such would be far less jerky.
That is what the code you mentioned does.
The first part verifies that the time that you wish to make the move in is not 0 (IE mediated), and it has moved previously as on the first move, the system has no way of knowing where the servos are).
So in my case above, suppose we are doing the example I mentioned above and aiLastPositions[iServo] is 90 and we passed in120 for the NewPosition and Time=500
The variable Delta would be set to 120-90 or 30
CntOfCycles would be set to 500/20 or 25
DeltaPerCycle would be 30/25 = 1 (Integer math, note in the code I mentioned there are things we can improve here).
So we can skip some of the code as DeltaPerCycle does not equal 0.
iPos starts off with the value 90
We then loop (while (cntofCycyles–) {
Sorry I should probably not used some of C’s short hand ways of saying things. It might be easier to understand if I said something like:
while(CntOfCycles!= 0) {
CntOfCycles= CntOfCycles-1;
…
the variable iPos is incremented by DeltaPerCycle. Again might be easier to understand if I wrote it like:
iPos = iPos + DeltaPerCycle;
We then write out each of these values and then delay by our cycle time in this case the default 20ms
Hope that helps
Kurt
