Home » BLOG

Création d’une carte avec le robot MRPiZ

Voici un exemple de création d’une carte avec le robot MRPiZ en utilisant le capteur de distance laser VL53L0X :

Le programme python de la création d’une carte :

  • Le robot tourne à 360° et lit le capteur de distance,
  • Enregistrement de l’orientation du robot et de la distance du capteur dans un fichier csv.
from mrpiZ_lib import *
import time, csv

NameFile = 'carte.csv'

def read_laser():
  p3_1 = proxSensor(3)
  p3_2 = proxSensor(3)
  p3_3 = proxSensor(3)
  p3_4 = proxSensor(3)
  p3_5 = proxSensor(3)
  p3 = (p3_1 + p3_2 + p3_3 + p3_4 + p3_5)/5
  return p3


############################################# MAIN

# wait init
time.sleep(2)

Fichier = open(NameFile,'w')
Robot_tourne = 0

# enable asv control
controlEnable()

while 1:
  # read laser distance sensor
  distance = read_laser()

  # read robot orientation
  orientation = robotPositionO()

  # write in file
  Fichier.write(str(distance))
  Fichier.write(";")
  Fichier.write(str(orientation))
  Fichier.write("\r\n")# nouvelle ligne

  if(Robot_tourne == 0):
    turnLeftDegree(8,360)
    Robot_tourne = 1

Fichier.close()
exit()

Le programme de lecture et d’affichage de la carte  :

import numpy as np
import math
import matplotlib.pyplot as plt
import numpy as np
import csv

liste_distance = []
liste_orientation = []

fichier = open("carte.csv", "r")

c = 0

# lecture fichier
while True:
  ligne = fichier.readline()
  if ligne =='':
    break # fin fichier
  # lecture de la distance (capteur distance)
  distance = ligne.split(';')[0]
  # lecture orientation
  orientation = ligne.split(';')[1]
  # correction de la distance par rapport au centre du robot et la position du capteur
  distance = float(distance) + 50

  # conversion en radian
  if float(orientation) > 0:
    orientation = (float(orientation)*3.14)/180

  liste_distance.append(float(distance))
  liste_orientation.append(float(orientation))
  c = c + 1

ax = plt.subplot(111, projection='polar')
ax.plot(liste_orientation, liste_distance)
ax.grid(True)
plt.show()

 

Exemple d’affichage : 

Imitation learning with MRPiZ robot

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[c] >= 0:
    cmd_right = 2.8*liste_mr[c]
    dir_right = 0
  elif liste_mr[c] < 0:
    cmd_right = 2.8*liste_mr[c]
    dir_right = 1
  else:
    dir_right = 0
    cmd_right = 0

  if liste_ml[c] >= 0:
    cmd_left = 2.8*liste_ml[c]
    dir_left = 0
  elif liste_ml[c] < 0:
    cmd_left = 2.8*liste_ml[c]
    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

STM32 – PWM complementary output exemple

GPIO_InitTypeDef GPIO_RGB_InitStruct;
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;

__HAL_RCC_GPIOB_CLK_ENABLE();

// PB7 - TIMER17
 GPIO_RGB_InitStruct.Pin = GPIO_PIN_7;
 GPIO_RGB_InitStruct.Mode = GPIO_MODE_AF_PP;
 GPIO_RGB_InitStruct.Pull = GPIO_NOPULL;
 GPIO_RGB_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
 GPIO_RGB_InitStruct.Alternate = GPIO_AF2_TIM17;
 HAL_GPIO_Init(GPIOB, &GPIO_RGB_InitStruct);


 __HAL_RCC_TIM17_CLK_ENABLE();

// Init TIMER17 for 800KHz frequency
 // 48MHz/(19+1)/(2+1) = 800Khz
 TIMER_RGB_InitStruct.Instance = TIM17;
 TIMER_RGB_InitStruct.Init.Prescaler = 0;
 TIMER_RGB_InitStruct.Init.CounterMode = TIM_COUNTERMODE_UP;
 TIMER_RGB_InitStruct.Init.Period = 59;

TIMER_RGB_InitStruct.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

HAL_TIM_Base_Init(&TIMER_RGB_InitStruct);

HAL_TIM_PWM_Init(&TIMER_RGB_InitStruct);

// Output Compare Configuration
 TIMER_RGB_OC_InitStruct.OCMode = TIM_OCMODE_PWM1;
 TIMER_RGB_OC_InitStruct.OCIdleState = TIM_OCIDLESTATE_RESET;
 TIMER_RGB_OC_InitStruct.OCNIdleState = TIM_OCNIDLESTATE_RESET;// ATTENTION ! TIM_OCNIDLESTATE_RESET
 TIMER_RGB_OC_InitStruct.Pulse = 1;// PWM 0 %
 TIMER_RGB_OC_InitStruct.OCPolarity = TIM_OCPOLARITY_HIGH;
 TIMER_RGB_OC_InitStruct.OCNPolarity = TIM_OCNPOLARITY_HIGH;// ATTENTION ! TIM_OCNPOLARITY_HIGH
 TIMER_RGB_OC_InitStruct.OCFastMode = TIM_OCFAST_DISABLE;

HAL_TIM_PWM_ConfigChannel(&TIMER_RGB_InitStruct, &TIMER_RGB_OC_InitStruct, TIM_CHANNEL_1);

HAL_TIMEx_PWMN_Start(&TIMER_RGB_InitStruct,TIM_CHANNEL_1);// ATTENTION HAL_TIMEx_PWMN_Start

Avancer vers un but avec gestion des obstacles

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

MRPiZ – odometry

 

 

 

import sys
from mrpiZ_lib import *
import time

time.sleep(5)

controlEnable()
forward_mm(10,220)
turnLeft_degree(10,90)
forward_mm(10,170)
turnLeft_degree(10,90)
forward_mm(10,170)
turnRight_degree(10,90)

forward_mm(10,290)
turnRight_degree(10,90)

forward_mm(10,375)

turnRight_degree(10,90)
forward_mm(10,450)

Envoyer des commandes SSH avec paramiko

Un script pour envoyer des commandes SSH avec paramiko.

Ce script permet de contrôler en SSH le robot MRPiZ à partir d’un PC.

#!/usr/bin/env python

import sys, paramiko
import time


hostname = '192.168.42.1' # IP du robot MRPiZ
password = 'raspberry'
username = 'pi'
port = 22

command = 'echo "#MF,30!" > /dev/ttyAMA0'

##################################################################################""

ssh = paramiko.SSHClient()
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
ssh.load_system_host_keys()
ssh.connect(hostname, port=port, username=username, password=password)


ssh.exec_command(command)

Dance de robots MRPiZ

 

La gestion des robots en parallèle est gérer par fabric (www.fabfile.org). Un outil SSH permettant d’envoyer plusieurs commandes SSH en parallèle. Chaques robots MRPiZ est identifié par une adresse IP unique:

  • IP du robot n°1: 192.168.1.41
  • IP du robot n°2: 192.168.1.37
  • IP du robot n°3: 192.168.1.47

Le script de contrôle des robots :

#!/usr/bin/env python
import fabric
from fabric.api import run, env, task
from fabric.api import *
import time


env.hosts = ['192.168.1.41', '192.168.1.37', '192.168.1.47']

env.user = 'pi'
env.password = 'raspberry'

@parallel
def hello():
 run('python /home/pi/MRPiZ/Dance.py')

Le script python pour les robots :

#!/usr/bin/env python

import sys
from mrpiZ_lib import *
import time

controlEnable()
while 1:
 forward_mm(13,120)
 back_mm(11,100)
 forward_mm(11,100)
 turnRight_degree(25,360)
 time.sleep(1)
 turnRight_degree(15,90)
 turnLeft_degree(15,180)
 turnRight_degree(15,90)
 back_mm(12,90)