Smooth servo motions

Hi Timothy,

Thank you so much for your detailed response. This answers my questions perfectly and I really want to give it a 10 out of 3. It's so nice to have some people to break down the code for me.

After reading this, I tried to understand how the angle updates are done. Here's my understanding,

For convenience, let's call the target angle $b$ and the current angle $a$, in

diff[i - offset] = currentAng[i] - target[i - offset] * angleDataRatio;

we compute the difference as $a-b$. In the following code

for (int s = 0; s <= steps; s++) {
	for (byte i = offset; i < DOF; i++) {
		float dutyAng = (target[i - offset] * angleDataRatio + (steps == 0 ? 0 : (1 + cos(M_PI * s / steps)) / 2 * diff[i - offset]));
		calibratedPWM(i, dutyAng);
	}
}

the angle updates are done in $steps$ steps, and each update computes dutyAng as

$$

b + (1 + cos(\pi*s / steps)) / 2 * (a-b)

= (1 - cos(\pi s / steps))/2 * b + (1 + cos(\pi  s / steps))/2 * a

$$

which is a weighted average of $a$ and $b$. However, the transition from $a$ to $b$ is not equally spaced. I assume this is a trick to make the motion more life-like.

Please correct me if it's not correct or something is missing.

And thank you again, Timothy.

(PS: It would be great if the forum can add support for LaTeX)

Best,

Jiarui

It's difficult to give a meaningful answer to this without additional information from you about your understanding of the "smooth motion" problem, level of programming experience, and servo control experiments you've done. As a result, I have to make a few inferences based upon your question.

I'm learning to work with servo motors.

You should then be able to directly control any servo of the bot. You can send the command to the servo to tell it what position it should move to. There are a number of Serial Protocol commands that perform this operation and you should be familiar with all of them and able to use them.

Since you are reading the source code I assume you have a basic understanding of the C++ language and can write a simple working sketch to control a servo. There are a number of tutorials online that cover this.

Smooth Motion

The inference here is that you understand that the basic servo commands produce a motion that is somewhat "spazmotic" and undesireable. What is the desired motion? To get an idea, write a test to move a servo from angle 0 to 90 degrees, 1 degree at a time. The add an arbitray delay between each step. Vary the degrees per step from 1 to 5 and see the results.

This should give you a clear idea of how to smooth the servo motion. It's dependent upon the number and size of the steps between servo positions and the time between each step, i.e., the speed.

Now we can read and better understand the transform template function.

For the purpose of comprehension we'll ignore the template nature of transform. That's not to say it's unimportant but we want to focus on the primary behavior of the function. The basic function interface is

void transform(
    T* target
    , byte angleDataRatio = 1
    , float speedRatio = 1
    , byte offset = 0
    , int period = 0
    , int runDelay = 8
) {

where T is nominally == int

The target array contains the "final" position angles for the servo motors.

The basic usage of the function is

// reaction.h
// ...
else if (T_INDEXED_SEQUENTIAL_ASC == token) {
	transform(targetFrame, 1, 1);
	delay(10);
}

Here, targetFrame is defined as an array of all servo motors

int targetFrame[DOF + 1];

and it is initialized with the current position of each servo. The command T_INDEXED_SEQUENTIAL_ASC alters targetFrame with positions for one or more servos. So the call

transform(targetFrame, 1, 1);

means "Move the servos from their current position to the new position using the default step increment and speed." Note that the call is slightly verbose, due to default argument values, and is equivalent to

transform(targetFrame);

If we outline the transform function we can eliminate inactive code:

void transform(
	T* target
	, byte angleDataRatio = 1
	, float speedRatio = 1
	, byte offset = 0
	, int period = 0
	, int runDelay = 8
) {
	if (0) { /*...*/ }
	else { /*...*/ }
}

Only the else clause is active. If we outline that code we see

else {
    int* diff = new int[DOF - offset]
	    , maxDiff = 0;
    for (byte i = offset; i < DOF; i++)
    {
        // calculate position differences ...
    }
    int steps = speedRatio > 0
	    ? int(round(maxDiff / 1.0 /*degreeStep*/ / speedRatio))
	    : 0;  //default speed is 1 degree per step
    for (int s = 0; s <= steps; s++) {
        for (byte i = offset; i < DOF; i++)
        {
            // translate new postion to PWM signal...
        }
        delay((DOF - offset) / 2);
    }
    delete[] diff;
}

The algorithm calculates the angular difference between the current servo position and the target position. This angle defines a segment of a circle that can be divided into descrete steps between the two positions. This is a trigonometric exercise that uses the cosine function to calculate the incremental angle (dutyAng) of each step. The incremental angle is then translated into a PWM signal sent to the servo.

Aside: Dr. Li, how would you rate this answer on a scale of 1-3 where

• 1 = excellent

• 2 = most excellent

• 3 = better than I could do.

More importantly, Jiarui, how would you rate this answer on scale of 1-3 where

• 1 = incomprehensible gibberish

• 2 = helpful

• 3 = very helpful