Hi,
I want to know how can I improve the following code so thymio does not get stuck correcting the position and what to do to run it smoothly when it is not detecting any objects in front of him and not bumping from one side to the other. Is there a way of correcting the code so it only avoid objects closer to him or is there a better algorithm to avoid obstacles. Thanks!!
var mot_l
var mot_r
- Variables for Braitenberg avoid routine
var avoid_l[5] = [2, 0, -1, -5, -10]
var avoid_r[5] = [-10, -5, -2, 0, 2]
var avoid_shift = 4
var avoid_offset = MAX_SPEED*2/5
callsub init
sub init
call leds.temperature(0,0)
sub braitenberg_avoid
# Use the vector dot product to calculate motor values
call math.dot(mot_l, avoid_l, prox.horizontal[0:4], avoid_shift)
mot_l += avoid_offset
call math.dot(mot_r, avoid_r, prox.horizontal[0:4], avoid_shift)
mot_r += avoid_offset
# If motor values are < -MAX_SPEED or > +MAX_SPEED fix them
call math.clamp(mot_l, -MAX_SPEED, mot_l, MAX_SPEED)
call math.clamp(mot_r, -MAX_SPEED, mot_r, MAX_SPEED)
# Write calculated motor values to Thymio variables to run motors
motor.left.target = mot_l
motor.right.target = mot_r
onevent prox
callsub braitenberg_avoid