var left_ir=0 var right_ir=0 var mode = MODE_STOP var angle=0 var acc_count = 0 var rotation=0 #### Activate Thymio onevent button.forward mode = MODE_COARSE call leds.bottom.left(0,0,0) call leds.bottom.right(0,0,0) call leds.top(0,0,0) #### Stop Thymio onevent button.backward mode = MODE_STOP call leds.bottom.left(0,0,0) call leds.bottom.right(0,0,0) call leds.top(0,0,0) motor.left.target = 0 motor.right.target = 0 #### Accelerometer event onevent acc #### Coarse mode to find approximately the equilibrium if mode == MODE_COARSE then call math.atan2(angle,acc[1],acc[2]) angle = angle/ANGLE_DIVISER_COARSE if prox.horizontal[0] > THRESHOLD_IR or prox.horizontal[4] > THRESHOLD_IR then motor.left.target = angle - rotation motor.right.target = angle + rotation else motor.left.target = angle motor.right.target = angle end if acc[1] < 0 then acc_count++ if acc_count > THRESHOLD_ACC_COUNT then mode = MODE_FINE call leds.bottom.left(0,0,32) call leds.bottom.right(0,0,32) call leds.top(0,0,32) end else acc_count = 0 end #### Fine mode to find precisely the equilibrium elseif mode == MODE_FINE then call math.atan2(angle,acc[1],acc[2]) angle = angle/ANGLE_DIVISER_FINE if prox.horizontal[0] > THRESHOLD_IR or prox.horizontal[4] > THRESHOLD_IR then motor.left.target = angle - rotation motor.right.target = angle + rotation else motor.left.target = angle motor.right.target = angle end end #### Proximity sensors event onevent prox left_ir = (prox.horizontal[4]-PROX_SUB)/PROX_DIV right_ir = (prox.horizontal[0]-PROX_SUB)/PROX_DIV rotation = left_ir - right_ir if prox.ground.delta[0] < THRESHOLD_IR_GROUND or prox.ground.delta[1] < THRESHOLD_IR_GROUND then mode = MODE_STOP call leds.bottom.left(32,0,0) call leds.bottom.right(32,0,0) call leds.top(32,0,0) motor.left.target = 0 motor.right.target = 0 end