• MotionProfile

[work in progress Author: Barry Carter Kick him if it is wrong.]

OpenServo Curve based motion

The OpenServo has an advanced motion profile feature that allows for many different types of line motion following. The motion engine is based on Hermite curves, and are extremely powerful.

Motion engine properties:

• Hermite
• Trapezoid
• Straight line
• Discontinuous motion

Each curve segment is defines as two keypoints, the start and the end. The OpenServo calculates the curve based on the Tout value of the first keypoint and the Tin on the last point.

The start point of the next motion segment is the same as the last point of the last motion segment. In order to create continuous motion (as per image below) you should set Tin and Tout to the same value. This means that the tangents will follow smoothly from one keyframe to the next.

Each keypoint has several properties that determine the type of motion profile that you are applying. The time delta (Td) the Position (P) and the tangent in (Tin) and tangent out (Tout)

As the speed is a measure of distance over time, the steepness of the gradient of the curve determines the speed of the movement. The steeper the slope, the faster the servo must move to perform the movement.

Features

• curve and line support
• 8 keyframe buffer
• Fast reaction time
• Accurate motion following.

Register layout & command functions

The register layout for the motion profile support is defined as such:

The command values are:

More about these later.

Time Delta (Td) – 0x18-0x19 (16 bit unsigned)

This is an indicator of the amount of time that the current keyframe should run for. This is specified in milliseconds (ms) as a 16bit unsigned integer.

For example: a 3 second movement would be 0x0BB8 (3000ms)

Position (P) – 0x1A-0x1B (16 bit unsigned)

The position register hold the destination position for the motor to move to.

Tangent in (Tin) & Tangent out (Tout) (6:10 fixed signed)

For normal smooth continuous curves, these values would be identical. If you are creating discontinuous movements, such as a sawtooth, you can send different Tin and Tout values to achieve this. Remember the Tin and Tout are applied to each keypoint and not to the total curve.

Calculating the Tangent in and Tangent out

The tangent is given as the gradient of the curve at the entry/exit point of the keypoint.

The tangent can be calculated by taking the gradient of the keypoint.

For example, to calculate the tin and tout of this curve, we do the following:

Take the gradient of the curve at the desired keypoint

Example conversion of a floating point precision tangent to a fixed math 6:10 signed tangent.

int16_t floatToFixed(float tangent) // Float to 6:10 signed fixed. {

• return (int16_t) (tangent * 1024.0);

}

It is normally a good idea to clip the tangent values to +-31.0 to help keep the OpenServo processing calculation times down.

Sending the commands

Once you have loaded up the correct data into the registers, you need to send a command to activate the curve

Reading the buffer size

You can read the remaining buffer space from the OpenServo by reading from the CURVE_BUFFER register. The OpenServo will hold 8 keypoints in its internal buffer so you have to be careful not to over fill this. If you try and write to a full buffer, data will be lost, and your new profile segment will be discarded.

Tying it all together

The sequence of events for sending a curve to the OpenServo is as follows:-

• Get your curve how you want it to look.
• Calculate the Time Delta (Td) from the beginning of the curve to the end.
• Take the gradient of the in and out tangent points for the beginning and end keypoints.
• Convert the keypoints to 6:10 signed math.

• Write the keypoint data to the OpenServo registers start 0x18

• Send the TWI_CMD_CURVE_MOTION_APPEND command
• Repeat for all new motion keyopints up to 8 in total.
• Monitor the CURVE_BUFFER register for any space to send the new keypoints

Advanced use… trapezoidal motion and non curve movement

The motion profile engine is capable of producing many differing movement types, from a straight line to a sawtooth like movement. All of these movement types can be used together without limitation.

In this example, the incoming and outgoing tangents are not the same. The tangents both follow a straight line between the first and last keypoint in each frame, creating a straight line from the hermite.

Using the same principle above, you can create trapeziodal motion. You need to make sure that the tangent lines are aligned in such a way as to create a line between the points. Again, the Tin and Tout of each frame is different.

You can also make the servo wait for long periods of time be sending a line that has no position change over the whole keyframe. You have to manipulate the tangent lines to make this happen, as was done in the trapezoid example.

Notes

It is worth keeping your curve segments small. If you encounter an anomaly in your robot movement, you can quickly clear the buffers and send revised motion profile movements. If the motion segments are large, it will take longer to react to any external forces you may encounter.

See Utilities for a Bezier curve following application.