Timed command interpolation algorithm

sendCommand is the last interpolated command as an integer. This is sent to the actuator, and initialized to 0.
lastCommandDouble is the last computed command, but this one in double precision, to keep the precision for the next computation. sendCommand is the nearest integer of lastCommandDouble.
lastCommandValue is the value of the last timed command, without interpolation.
lastCycleTime is the last time when this actuator has run through the algorithm.
lastCommandTime is the exact time of the last command.

For each actuator:

do Look for the next timed command (they are ordered by time) If there is no one Set the lastCycleTime to current time else, if there is one diff0 = time difference between the current time and the command time if the command time is > to the current time (passed command), then stop loop after this one if the command time is >= to the current time (passed command) apply it, that means sendCommand, lastCommandDouble and lastCommandValue are now equal to the timed command value ; lastCommandTime is equal to the timed command, lastCycleTime is equal to the current time. and remove this timed command from the list else (future command) if the lastCommandTime is between the current time and the last cycle time (that means if the last command has happened between the last cycle and this one) VarlastTime = lastCommandTime VarlastCommand = lastCommandValue else (if the last command was before the last cycle) VarlastTime = lastCycleTime VarlastCommand = lastCommandDouble diffTime1 = time between the current time and the VarlastTime diffTime2 = time between the new command time and the VarlastTime VarCommand = ((value of the new command - VarlastCommand)*diffTime1)/diffTime2 + VarlastCommand sendCommand = nearest rounding of VarCommand lastCommandDouble = VarCommand lastCycleTime = current time loop

Warning:	Sending 2 or more timed commands at exactly the same date (in ms) with different values will result in unpredictable result.

First example:

We suppose that there is a DCM cycle every 10ms.

The command 1 was sent to (10,10). (first number is time, second is value, like degrees for joints).

At t=30ms (or between 20 and 30) the DCM received another command at (80,40).

command value

Here is the computation of the next cycles:

t = 30ms
diffTime1 = 30 - 20 = 10. diffTime2 = 80 - 20 = 60

VarCommand = ((40 -10)*10)/60 + 10 =15
t = 40ms
diffTime1 = 40 - 30 = 10. diffTime2 = 80 - 30 = 50

VarCommand = ((40 -15)*10)/50 + 15 = 20
t = 50ms
diffTime1 = 50 - 40 = 10. diffTime2 = 80 - 40 = 40

VarCommand = ((40 -20)*10)/40 + 20 = 25
t = 60ms
diffTime1 = 60 - 50 = 10. diffTime2 = 80 - 50 = 30

VarCommand = ((40 -25)*10)/30 + 25 = 30
t = 70ms
diffTime1 = 70 - 60 = 10. diffTime2 = 80 - 60 = 20

VarCommand = ((40 -30)*10)/20 + 30 = 35
t = 80ms
diffTime1 = 80 - 70 = 10. diffTime2 = 80 - 70 = 10

VarCommand = ((40 -35)*10)/10 + 35 = 40

Then the value stays at 40 for the next cycles, until another command is sent.

Second example:

We suppose that there is a DCM cycle every 10ms.

At t=10ms (or between 0 and 10), the DCM received 4 commands for this actuator: (15, 10) (25, 30) (45,20) and (65, 0).

command value

Here is the computation of the next cycles:

t = 10ms
diffTime1 = 10 - 0 = 10. diffTime2 = 15 - 0 = 15

VarCommand = ((10 -0)*10)/15 + 0 = 6.66
t = 20ms
diffTime1 = 20 - 15 = 5. diffTime2 = 25 - 15 = 10

VarCommand = ((30 - 10)*5)/10 + 10 = 20
t = 30ms
diffTime1 = 30 - 25 = 5. diffTime2 = 45 - 25 = 20

VarCommand = ((20 - 30)*5)/20 + 30 = 27.5
t = 40ms
diffTime1 = 40 - 30 = 10. diffTime2 = 45 - 30 = 15

VarCommand = ((20 - 27.5)*10)/15 + 27.5 = 22.5
t = 50ms
diffTime1 = 50 - 45 = 5. diffTime2 = 65 - 45 = 20

VarCommand = ((0 - 20)*5)/20 + 20 = 15
t = 60ms
diffTime1 = 60 - 50 = 10. diffTime2 = 65 - 50 = 15

VarCommand = ((0 - 15)*10)/15 + 15 = 5
t = 70ms and above
VarCommand = 0

<< Previous

Next >>