Contrôle à distance du HAT-MDD10

August 17, 2019 sysop Programmation, Projets, WiFi 0

Introduction

Nous avons vu précédemment la méthode à utiliser pour faire varier la puissance et le sens de rotation d’un moteur. l faut maintenant être capable de moduler ces valeurs à distance. Le langage python offre tous les outils nécessaires pour y arriver.

Pour ces exemples, j’ai fait le choix d’utiliser UDP comme protocole de communication. TCP est une alternative acceptable malgré une certaine latence dans la transmission du signal de contrôle. UDP reste un choix intéressant pour sa simplicité et sa rapidité. Le développeur devra toutefois garder à l’esprit le potentiel d’erreur de transmissions plus élevé.

Programmes

Je propose au lecteur les 3 exemples suivants ;

1 – Un serveur de contrôle moteur utilisant le protocole UDP. Ce serveur est exécuté sur le véhicule robotique mobile.

Code source : Server_UDP.zip

2 – Un client UDP en mode console qui utilise le clavier comme interface utilisateur. Le client est exécuté sur le portable faisant office de station de contrôle.

Code source : Client_UDP_keyboard.zip

3 – Un client UDP en mode console qui utilise une manette de contrôle de type PS3. Le client est exécuté sur le portable faisant office de station de contrôle.

Code source : Client_UDP_joystick.zip

Environnement de développement

Le serveur UDP fonctionne sous Raspbian version Stretch tandis que les programmes clients utilisent Ubuntu 1804LTS comme système d’opération. Le client utilisant le clavier va se servir de la librairie python pynput pour capturer les valeurs entrées au clavier. Le client qui utilise la manette de type PS3 va dépendre de la librairie pygame pour accéder aux commandes de l’utilisateur. Ces deux librairies devront être installées préalablement. La manette compatible PS3 utilisée est le modèle Logitech F310

Construction des programmes

Pseudo-code du serveur UDP.

1 - Initialiser le contrôleur de moteur.
2 - Ouvrir un port UDP à l'écoute des messages de contrôle.
3 - Recevoir un message de contrôle.
4 - Décoder les données reçues en valeurs applicables.
5 - Appliquer les valeurs reçues au contrôleur de moteur.
6 - Recommencer à l'étape 3

Pseudo-code du client UDP.

1 - Initialiser l'interface de contrôle (clavier, manette PS3)
2 - Capturer les intrants de l'utilisateur.
3 - Formater les intrants en message de contrôle.
4 - Envoyer le message de contrôle au serveur UDP.
5 - Recommencer à l'étape 2

Exemple 1 – Serveur UDP 

#!/usr/bin/python3

import RPi.GPIO as GPIO
from time import sleep
import sys
import os
import socket

# Global variables
CONTROL_IP = '0.0.0.0'
CONTROL_PORT = 10001
MOTOR_VALUE_MIN = 0
MOTOR_VALUE_MAX = 100
DEBUG = False

# GPIO settings
GPIO.setmode(GPIO.BCM) # GPIO numbering
GPIO.setwarnings(False) # enable warning from GPIO
AN1 = 12 # set power pin for left motor
DIG1 = 26 # set direction pin for left motor
AN2 = 13 # set power pin for right motor
DIG2 = 24 # set direction pin for right motor
GPIO.setup(AN1, GPIO.OUT) # set power pin AN1 as output
GPIO.setup(AN2, GPIO.OUT) # set power pin AN2 as output
GPIO.setup(DIG1, GPIO.OUT) # set direction pin DIG1 as output
GPIO.setup(DIG2, GPIO.OUT) # set direction pin DIG2 as output
p1 = GPIO.PWM(AN1, MOTOR_VALUE_MAX) # init power pin p1
p2 = GPIO.PWM(AN2, MOTOR_VALUE_MAX) # init power pin p2

# Functions
def go_forward(speed):
    p1.start(speed)
    GPIO.output(DIG1, GPIO.LOW)
    p2.start(speed)
    GPIO.output(DIG2, GPIO.LOW)

def go_reverse(speed):
    p1.start(speed)
    GPIO.output(DIG1, GPIO.HIGH)
    p2.start(speed)
    GPIO.output(DIG2, GPIO.HIGH)

def pivot_right(speed):
    p1.start(speed)
    GPIO.output(DIG1, GPIO.HIGH)
    p2.start(speed)
    GPIO.output(DIG2, GPIO.LOW)

def pivot_left(speed):
    p1.start(speed)
    GPIO.output(DIG1, GPIO.LOW)
    p2.start(speed)
    GPIO.output(DIG2, GPIO.HIGH)

def stop():
    p1.start(MOTOR_VALUE_MIN)
    p2.start(MOTOR_VALUE_MIN)

def start_control_server(ip,port):
    MAX_BYTES = 1024
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((ip, port))
    print("[MSG]> Server listening at {}".format(sock.getsockname()))
    try:
        while True:
            # Receive data from client
            data, address = sock.recvfrom(MAX_BYTES)
            if data:
                # Decode client data
                data = data.decode('utf-8')
                # Get values from client data
                k_up,k_down,k_left,k_right = data.split(',')
                print("{},{},{},{}".format(k_up,k_down,k_left,k_right))
                # Apply values to motor control
                if (int(k_up)): go_forward(MOTOR_VALUE_MAX)
                elif (int(k_down)): go_reverse(MOTOR_VALUE_MAX)
                elif (int(k_left)): pivot_left(MOTOR_VALUE_MAX)
                elif (int(k_right)): pivot_right(MOTOR_VALUE_MAX)
                else: stop()
    finally:
        sock.close()
        GPIO.cleanup()

# Main
if __name__ == '__main__':
    os.system('clear')
    try:
        start_control_server(CONTROL_IP,CONTROL_PORT)
    except KeyboardInterrupt:
        print("Exit")
        sys.exit()

Exemple 2 – Client UDP utilisant le clavier.

#!/usr/bin/python3

from pynput import keyboard
from time import sleep
import os
import sys
import socket

ROVER_CONTROL_IP = '192.168.99.1'
ROVER_CONTROL_PORT = 10001
MOVE_DETECTED = False

# send UDP data to server
def UDP_send_data(data,ip,port):
    error_code = 0
    UDP_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    UDP_socket.sendto(data,(ip,port))
    return error_code

# pynput function, detect key pressed
def on_press(key):
    global control_msg
    global MOVE_DETECTED
    if key == keyboard.Key.up:
        control_msg = '100,0,0,0'
        MOVE_DETECTED = True
    elif key == keyboard.Key.down:
        control_msg = '0,100,0,0'
        MOVE_DETECTED = True
    elif key == keyboard.Key.left:
        control_msg = '0,0,100,0'
        MOVE_DETECTED = True
    elif key == keyboard.Key.right:
        control_msg = '0,0,0,100'
        MOVE_DETECTED = True

# pynput function, detect key released 
def on_release(key):
    global MOVE_DETECTED
    MOVE_DETECTED = False

#start keyborad control client
def start_control_client(robot_ip,robot_port): 
    global control_msg
    global MOVE_DETECTED
    print("[MSG]> Start control client")
    print("[MSG]> Remote server: {}:{}".format(robot_ip,robot_port) ) 
    print("[MSG]> Ctrl-C to exit")
    try:
        # init keyboard capture, run in background
        listener = keyboard.Listener(on_press=on_press,on_release=on_release)
        listener.start()
        control_msg = ""
        while True:
            # no movement detected, set to stop
            if MOVE_DETECTED != True:
                control_msg = '0,0,0,0'
            # movement detected, send values to server
            if control_msg != "":
                print("{}[KEY]> {}".format('\b\b\b\b',control_msg))
                try:
                    UDP_send_data(control_msg,robot_ip, robot_port) 
                except:
                    print("[MSG]> Network error")
                    sys.exit()
            sleep(0.01) 
    finally:
        pynput.keyboard.Listener.stop

# Main
if __name__ == "__main__":
    try:
        start_control_client(ROVER_CONTROL_IP,ROVER_CONTROL_PORT)
    finally:
        print("\b\b[MSG]> Exit")
        sys.exit()

Exemple 3 – Client UDP utilisant une manette PS3

#!/usr/bin/python3

import socket
import pygame
import sys

ROVER_CONTROL_IP = '192.168.99.1'
ROVER_CONTROL_PORT = 10001

# UDP_send_data(data,ip,port)
def UDP_send_data(data,ip,port): 
    error_code = 0
    UDP_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    UDP_socket.sendto(data,(ip,port))
    return error_code

# data_to_pwm(axis_1,axis_2)
def data_to_pwm(axis_1,axis_2):
    deviation = 0.01
    LF,LR,RL,RR = 0,0,0,0
    if axis_1 < 0:
        LF = abs(int (axis_1 * 100))
    elif axis_1 > 0:
        LR = abs(int ((axis_1 + deviation ) * 100))
    if axis_2 < 0:
        RL = abs(int (axis_2 * 100))
    elif axis_2 > 0:
        RR = abs(int ((axis_2 + deviation) * 100))
    return (LF,LR,RL,RR)

# start joystick control client
def start_control_client(device_id,robot_ip,robot_port): 
    print("[MSG]> start_control_client()")
    # init joystick values
    interference_level = 99
    LF,LR,RL,RR = 0,0,0,0 
    #init pygame 
    pygame.init()
    pygame.joystick.init()
    clock = pygame.time.Clock()
    #init joystick 
    try:
        j = pygame.joystick.Joystick(device_id) # create a joystick instance
        j.init() # init instance
        print("[MSG]> Enabled joystick: {}".format(j.get_name()))
    except pygame.error:
        print("[MSG]> no joystick found")
    while True:
        # get joystick values
        for e in pygame.event.get(): 
        control_msg = ""
        left_j_value = 0
        right_j_value = 0 
        left_j_value = j.get_axis(1)
        right_j_value = j.get_axis(2) 
        #format control message from joystick values
        if (left_j_value or right_j_value):
            LF,LR,RL,RR=0,0,0,0
            LF,LR,RL,RR = data_to_pwm(left_j_value,right_j_value) 
            if LF < interference_level: LF=0
            if LR < interference_level: LR=0
            if RL < interference_level: RL=0
            if RR < interference_level: RR=0 
            control_msg = '{},{},{},{}'.format(LF,LR,RL,RR)  
        # send control message to server
        if control_msg != "": 
            print "[JOY]> {0}".format(control_msg)
            UDP_send_data(control_msg,robot_ip, robot_port)
        clock.tick(100) 

# main
if __name__ == "__main__":
    try:
        start_control_client(0,ROVER_CONTROL_IP,ROVER_CONTROL_PORT)
    finally:
        print("[MSG]> Exit")
        sys.exit()