Imitation learning exemple with the MRPiZ robot :

• record the speed of the two motors
• repead the speed of the motors

 

The python code :

from mrpiZ_lib import *
import time


# acquisition frequency
TIME_ACQ = 0.1

# acquisition time
TEMPS_ACQ = 200

liste_mr = []
liste_ml = []

# main program

# record loop
c = 0

motorsDisable()

time.sleep(1)

while c < TEMPS_ACQ:
  ml = motorLeftSpeed() # read left motor speed
  mr = motorRightSpeed() # read right motor speed
  liste_mr.append(mr)
  liste_ml.append(ml)
  time.sleep(TIME_ACQ)
  print ml, mr, c
  c = c + 1


print "END RECORD -----------"
time.sleep(2)

c=0
cmd_right=0
cmd_left=0
dir_right = 0
dir_left = 0

# H-bridge enable
motorsEnable()

##########################################################

# imitation
while c < TEMPS_ACQ:
  if liste_mr >= 0:
    cmd_right = 2.8*liste_mr
    dir_right = 0
  elif liste_mr < 0:
    cmd_right = 2.8*liste_mr
    dir_right = 1
  else:
    dir_right = 0
    cmd_right = 0

  if liste_ml >= 0:
    cmd_left = 2.8*liste_ml
    dir_left = 0
  elif liste_ml < 0:
    cmd_left = 2.8*liste_ml
    dir_left = 1
  else:
    cmd_left = 0
    dir_left = 0
  print cmd_left, cmd_right, c
  c= c + 1
  # motors commands
  motorRight(dir_right,abs(cmd_right))
  motorLeft(dir_left,abs(cmd_left))
  time.sleep(TIME_ACQ)

stop()
# end