![HATMDD10_controle_distance_p1_img1](https://framboiserobot.ca/wp-content/uploads/2019/08/HATMDD10_controle_distance_p1_img1-678x381.jpg)
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()