Basic odometry

[davidjsherman]

Odometry on the Thymio II will always be an approximation. However the odometry calculation in thymio_motion.aesl (see formula) has proven to be useful, especially in Scratch integration. The formula used, from [Lucas 2000], is inexpensive and appropriate for the Thymio II.

A simple addition to the firmware could perform this systematically on a variable odo[3] containing x, y, theta. If necessary a further value could be reserved for calibration.

This should be more accurate than the Aesl code it would replace (lines 167–174), in particular by avoiding precision and rounding errors.

An optimized version of this code, updated at 100 Hz, could be added to motor_new_analog.

Users would write to the odo variable to set a known initial position.

[davidjsherman]

The Aesl code to replace is:

onevent motor
odo.delta = (motor.right.target + motor.left.target) / 2
call math.muldiv(tmp[0], (motor.right.target - motor.left.target), 3406, 10000)
odo.theta += tmp[0]
call math.cos(tmp[0:1],[odo.theta,16384-odo.theta])
call math.muldiv(tmp[0:1], [odo.delta,odo.delta],tmp[0:1], [32767,32767])
odo.x += tmp[0]/45
odo.y += tmp[1]/45

Maybe using motor.*.speed rather than motor.*.target would be more accurate, but because of the noise in speed it might be better to use a low-pass filter on target–speed as suggested by [Wang 2013]