[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:

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.

keyframe.png

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.

overview.png

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

Register layout & command functions

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

0x18 & 0x19

unsigned 16bit time delta from previous keypoint

0x1A & 0x1B

unsigned 16bit position

0x1C & 0x1D

signed fixed point 6:10 bit in-tangent

0x1E & 0x1F

signed fixed point 6:10 bit out-tangent

The command values are:

0x91 TWI_CMD_CURVE_MOTION_ENABLE

0x92 TWI_CMD_CURVE_MOTION_DISABLE

0x93 TWI_CMD_CURVE_MOTION_RESET

0x94 TWI_CMD_CURVE_MOTION_APPEND

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:

curve2.png

Take the gradient of the curve at the desired keypoint

tancalc.png

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. {

}

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

0x91

TWI_CMD_CURVE_MOTION_ENABLE

This enabled the motion profile support. Any positions that are written while in this mode will be ignored.

0x92

TWI_CMD_CURVE_MOTION_DISABLE

Disable the motion profile support and resume normal position operations.

0x93

TWI_CMD_CURVE_MOTION_RESET

Clear the queue of buffered curved.

0x94

TWI_CMD_CURVE_MOTION_APPEND

Append the new data previously written to the curve processing buffer.

Reading the buffer size

0x17

REG_CURVE_BUFFER

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:-

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.

sawtooth.png

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.

trapezoid.png

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.

MotionProfile (last edited 2012-10-12 19:51:18 by localhost)