Home » BLOG » Archives for macerobotics

Author: macerobotics

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)