Un programme python pour faire avancer le robot MRPiZ vers un but avec la prise en compte des obstacles.

#!/usr/bin/env python
# Mace Robotics

from mrpiZ_lib import *
import time

# erreur odometrie
error_odo = 20

# sleep 2 secondes
time.sleep(2)

# but en millimetre
goal_robot = 200

# activer controle
controlEnable()

# lecture du capteur avant
sensor_p3 = proxSensor(3)

# lecture position du robot (axe X)
position_robot = robotPositionX()

distance_robot = goal_robot

while (position_robot < goal_robot-error_odo):
  forwardmm(10,distance_robot)# avancer vers le but
  sensor_p3 = proxSensor(3)# lecture capteur
  time.sleep(0.2) # pause 200 ms
  position_robot = robotPositionX()# lecture position robot
 
  # si obstacle 
  if (sensor_p3 < 100):
    stop()# arret du robot
    distance_robot = goal_robot - position_robot # erreur sur la distance


#end