Open davidjsherman opened 6 years ago
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]
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.