Makers – Robot Hack – Jour 2

Aujourd’hui on va tenter de détourner l’électronique du robot-aspirateur et essayer de s’interfacer avec via un Raspberry-Pi et un Arduino home-made (un ATmega328P-PU alimenté par le 3.3v du Pi, sur son oscillateur 8MHz interne).
L’objectif, c’est transformer l’ex-aspirateur-debilos en robot radio-commandé par Wi-Fi (via SSH) et par un joypad de Xbox360, pour commencer gentiment et vérifier que tout fonctionne comme prévu. Une fois cette étape passée, il ne restera (presque) plus qu’à s’occuper des capteurs et de la logique interne du robot…

Après examen du PCB principal, le plus rapide, pour le moment, semble de détourner les 2 moteurs, récupérer le 9V des moteurs directement sur un de leurs connecteurs, et les piloter avec 2 transistors via l’ATmega, via le Pi.
On ne dispose pas de H-bridge dans nos composants de récup’ pour remplacer les transistors, d’où ce choix. Un seul composant H-bridge permettrait de pouvoir contrôler la vitesse de chaque moteur, et dans les 2 sens. Avec notre solution des transistors, on sera pour le moment bloqué avec uniquement la marche avant pour chaque moteur, mais c’est déjà pas si mal pour un début. A suivre pour une éventuelle solution alternative…

– on bloque le capteur optique avec un bout de carton ou autre pour avoir un 9V continu sur les 2 connecteurs moteurs,
– le 9V des moteurs est donc récupéré d’un des connecteurs moteurs,
– on connecte 2 transistors aux moteurs et à l’ATmega, selon le schéma suivant :
bot_1_bb(les résistances font 330ohms mais ce n’est pas critique)
– on connecte les pins 1 (Rx) et 2 (Tx) de l’ATmega328 aux pins 8 (TxD) et 10 (RxD) du Raspberry. Puisque l’ATmega tourne sur le 3.3V du Pi, on a pas à s’inquiéter de différences entre niveaux logiques, tout est sur une base de 3.3V,
– la pile représente le +9V qui est repiqué sur l’un des connecteurs moteurs, sur le PCB d’origine,
– on connecte le Pi sur sa batterie dédiée pour le moment. Si possible, on l’alimentera plus tard directement sur la batterie du robot.
– on relie le tout à une masse commune

Du côté du Raspberry Pi, qui est connecté à notre réseau local via Wi-Fi, on va utiliser plusieurs composants, qui seront les bases du « cerveau » de notre robot :
– un premier script en python, le « server_udp.py », crée un serveur UDP, attend des commandes de type « 7/5/200/ » (explication en dessous) sur un port prédéfini et les renvoi directement à l’ATmega via les GPIO UART
– un second script python « client_udp.py » nous permettra -temporairement- de piloter le robot à distance grace à un joypad xbox360 dans un premier temps, histoire de vérifier que l’électronique et la liaison Raspi/ATmega fonctionnent bien.

Je vous fais grâce d’un dernier script python, pas super intéressant, qui fait exactement la même chose que client_udp.py mais via une invite en ligne de commandes (donc le robot est pilotable par SSH de cette manière).

Sur l’ATmega328, le code attend des commandes de type « 7/5/200/ » sur ses ports GPIO UART, les traduit en instructions, et renvoi le résultat le cas échéant. Par exemple :
– « 1/1/2/1/ » -> write / analog / pin 2 / HIGH
– « 1/1/4/0/ » -> write / analog / pin 4 / LOW
– « 2/2/3/ » -> read / digital / pin 3
– « 7/3/255/ » -> write PWM / pin 3 / value 255
– etc…
Ce code (beta, pas testé à 100%) est censé pouvoir gérer les lectures/ecritures analog/digital, le controle PWM, le controle servos (non testé), les shift register 595 et les sensors ultrasons pour le moment. Pour les curieux qui décortiqueront le code, une commande spéciale « 42 » est envoyée par le Pi et oblige l’ATmega à répondre « arduino », ce qui permet aux 2 entités, Pi et Atmega, de s’identifier en début de dialogue (les geeks comprendront la référence ^^).
C’est promis, je ferai un article complet et détaillé sur le sujet.

L’ATmega328P-PU est cablé puis programmé via un Arduino Uno chargé avec le code Arduino-ISP, puis installé sur son petit PCB dédié, connecté au Pi. Un article sera également rédigé d’ici peu au sujet de cette méthode de programmation d’un micro-controlleur.

Après la mise en place (temporaire) et le branchement de tout ce petit monde, ca donne ce résultat :

Il ne reste plus qu’à tester le tout…

Cooooool !!! Bon maintenant qu’on a un tank télé-commandé super cool avec plein de gadgets (j’abuse à peine ^^), il est temps de bosser sur son cerveau…

Voici les 3 scripts dont j’ai parlé un peu au dessus. Ils sont loin d’être mis au propre, puisque je poste au fur et à mesure des avancées, et on doit aller vite, très vite pour tout faire. En cas de problème, n’hésitez pas à demander en commentaires !

#include <Servo.h>
#include <RCSwitch.h>

unsigned long serialdata;
int inbyte;
int servoPose;
int servoPoses[80] = {};
int attachedServos[80] = {};
int servoPin;
int pinNumber;
int sensorVal;
int analogRate;
int digitalState;
Servo myservo[] = {};

int led = 13;
int blinkdelay = 1;

// Shift registers
int srd;
int srl;
int src;
int srdata;

// rf 433
RCSwitch mySwitch = RCSwitch();
int rf433Pin;
int rf433Data1;
int rf433Data2;

void setup()
{
  Serial.begin(9600);
  pinMode(led, OUTPUT);
  digitalWrite(led, LOW);
}

void blinkLED(byte targetPin, int numBlinks, int blinkRate) {
  for (int i=0; i < numBlinks; i++) {
    digitalWrite(targetPin, HIGH);   // sets the LED on
    delay(blinkRate);                     // waits for blinkRate milliseconds
    digitalWrite(targetPin, LOW);    // sets the LED off
    delay(blinkRate);
  }
}

void tobin(int valeur, int digit, int nombre[])
{
      int i;
      int temp;

      temp=valeur;
      for (i=0;i<digit;i++){   // itère pour le nombre de digit attendus             nombre[i]=temp & 1;  // prend le LSB et le sauve dans nombre             temp = temp >> 1;    // décalage d'un bit sur la droite
      }
}

void loop()
{
  getSerial();
  blinkLED(led, 1, blinkdelay); 
  switch(serialdata)
  {
  // PIN WRITE
  case 1:
    {
      //blinkLED(led, 1, blinkdelay);
      //analog digital write
      getSerial();
      switch (serialdata)
      {
      case 1:
        {
          //analog write
          getSerial();
          pinNumber = serialdata;
          getSerial();
          analogRate = serialdata;
          pinMode(pinNumber, OUTPUT);
          analogWrite(pinNumber, analogRate);        
          pinNumber = 0;
          break;
        }
      case 2:
        {
          //digital write
          getSerial();
          pinNumber = serialdata;
          getSerial();
          digitalState = serialdata;
          pinMode(pinNumber, OUTPUT);
          if (digitalState == 0)
          {
            digitalWrite(pinNumber, LOW);
          }
          if (digitalState == 1)
          {
            digitalWrite(pinNumber, HIGH);
          }
          pinNumber = 0;
          break;

        }
     }
     break; 
    }
    // PIN READ
    case 2:
    {
      //blinkLED(led, 1, blinkdelay);
      getSerial();
      switch (serialdata)
      {
      case 1:
        {
          //digital read
          getSerial();
          pinNumber = serialdata;
          pinMode(pinNumber, INPUT);
          sensorVal = digitalRead(pinNumber);
          Serial.println(sensorVal);
          sensorVal = 0;
          pinNumber = 0;
          break;
        }
      case 2:
        {
          //analog read
          getSerial();
          pinNumber = serialdata;
          pinMode(pinNumber, INPUT);
          sensorVal = analogRead(pinNumber);
          Serial.println(sensorVal);
          sensorVal = 0;
          pinNumber = 0;
          break;
        } 
      }
      break;
    }
    // SERVO
    case 3:
    {
      //blinkLED(led, 1, blinkdelay);
      getSerial();
      switch (serialdata)
      {
        case 1:
        {
           //servo read
           getSerial();
           servoPin = serialdata;
           Serial.println(servoPoses[servoPin]);
           break;
        }
        case 2:
        {
           //servo write
           getSerial();
           servoPin = serialdata;
           getSerial();
           servoPose = serialdata;
           if (attachedServos[servoPin] == 1)
           {
             myservo[servoPin].write(servoPose);
           }
           if (attachedServos[servoPin] == 0)
           {
             Servo s1;
             myservo[servoPin] = s1;
             myservo[servoPin].attach(servoPin);
             myservo[servoPin].write(servoPose);
             attachedServos[servoPin] = 1;
           }
           servoPoses[servoPin] = servoPose;
           break;
        }
        case 3:
        {
          //detach
          getSerial();
          servoPin = serialdata;
          if (attachedServos[servoPin] == 1)
          {
            myservo[servoPin].detach();
            attachedServos[servoPin] = 0;  
          }
        }
      }
    break;
    }
    // Shift register
    case 4:
    {
      //blinkLED(led, 1, blinkdelay);
      getSerial();
      srd = serialdata;
      getSerial();
      srl = serialdata;
      getSerial();
      src = serialdata;
      getSerial();
      srdata = serialdata;
      pinMode(srd, OUTPUT);
      pinMode(srl, OUTPUT);
      pinMode(src, OUTPUT);
      digitalWrite(srl, LOW);         
      shiftOut(srd, src, MSBFIRST, srdata); 
      digitalWrite(srl, HIGH); 
      // Serial.println(srdata);
      break;
    }
    // prise RF433 send
    case 5:
    {
      //blinkLED(led, 1, blinkdelay);
      getSerial();
      rf433Pin = serialdata;
      Serial.println(rf433Pin);
      getSerial();
      rf433Data1 = serialdata;
      Serial.println(rf433Data1);
      getSerial();
      rf433Data2 = serialdata;
      Serial.println(rf433Data2);
      getSerial();
      mySwitch.enableTransmit(rf433Pin);
      switch (serialdata)
      {
        case 0:
        {
          mySwitch.switchOff(rf433Data1, rf433Data2);
        }
        case 1:
        {
          mySwitch.switchOn(rf433Data1, rf433Data2);
        }
      }
      break;
    }
    // HC-SR04
    case 6:
    {
      getSerial();
      int trigPin = serialdata;
      getSerial();
      int echoPin = serialdata;
      long lecture_echo; 
      // init pins
      pinMode(trigPin, OUTPUT); 
      digitalWrite(trigPin, LOW); 
      pinMode(echoPin, INPUT); 

      digitalWrite(trigPin, HIGH); 
      delayMicroseconds(10); 
      digitalWrite(trigPin, LOW);
      lecture_echo = pulseIn(echoPin, HIGH); 
      Serial.println(lecture_echo);
      break;
    }
    // DC Motor
    case 7:
    {
      getSerial();
      int pwmPin = serialdata;
      getSerial();
      int pwmVar = serialdata;
      pinMode(pwmPin, OUTPUT);
      analogWrite(pwmPin, pwmVar);
      break;
    }
      // Identification
    case 42:
    {
      //blinkLED(led, 3, 10);
      Serial.println("arduino");
      break;
    }
  }
}

long getSerial()
{
  serialdata = 0;
  while (inbyte != '/')
  {
    inbyte = Serial.read(); 
    // inbyte = Serial.read()-'0';
    if (inbyte > 0 && inbyte != '/')
    {

      serialdata = serialdata * 10 + inbyte - '0';
    }
  }
  inbyte = 0;
  return serialdata;
}

server_udp.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import os
import threading
from datetime import datetime, time
import math
from time import sleep

DEBUG = 1

# --- Le serveur UDP ---

# Importe socket..
from socket import *

# Variables
host = "localhost"
port = 2074
buf = 128
addr = (host,port)

# ----------------------

# --- arduino - serial ---
import serial

locations=['/dev/ttyAMA0', '/dev/ttyUSB0','/dev/ttyUSB1','/dev/ttyUSB2','/dev/ttyUSB3',
'/dev/ttyS0','/dev/ttyS1','/dev/ttyS2','/dev/ttyS3']
get_uart_delay = 5
device = ""
found_device = ""
arduino = ""
# -----------------------

write_dir = "/run/shm/variables"

class MyTimer:
    def __init__(self, tempo, target, args= [], kwargs={}):
        self._target = target
        self._args = args
        self._kwargs = kwargs
        self._tempo = tempo

    def _run(self):
        self._timer = threading.Timer(self._tempo, self._run)
        self._timer.start()
        self._target(*self._args, **self._kwargs)

    def start(self):
        self._timer = threading.Timer(self._tempo, self._run)
        self._timer.start()

    def stop(self):
        self._timer.cancel()

def get_uart_device():
	global device
	global found_device
	global arduino

	for device in locations: 
		if DEBUG == 1:
			print "udp2uart : Trying...",device
		try:
			arduino = serial.Serial(device, 9600, timeout=2, bytesize=8, parity='N', stopbits=1)
			try:
				arduino.write("42/")
				# pygame.time.wait(0.1)
				answer = format(arduino.readline().strip('\r\n').strip())
				var_type = type(answer)
				#print(var_type)
				if ("arduino" in answer):
					arduino.close()
					arduino.open()
					found_device = device
					if DEBUG == 1:
						print("udp2uart : Arduino trouvé sur  "+device+"")
					os.system("echo "+found_device+" > "+write_dir+"/arduino")
					break
				else:
					print("udp2uart : mauvaise réponse de "+device+" ("+answer+")")
					found_device = ""
					os.system("echo "+found_device+" > "+write_dir+"/arduino")
			except:
				found_device = ""
				os.system("echo "+found_device+" > "+write_dir+"/arduino")
				if DEBUG == 1:
					print("udp2uart : echec de l'envoi des données à "+device+"")
		except:
			found_device = ""
			os.system("echo "+found_device+" > "+write_dir+"/arduino")
			if DEBUG == 1:
				print("udp2uart : echec de la connexion à "+device+"")

def trunc(f, n):
	'''Truncates/pads a float f to n decimal places without rounding'''
	return ('%.*f' % (n + 1, f))[:-1]

def uart_dialog(cmd,source_addr):
	command = cmd.split('/')
	if DEBUG == 1:		
		# pin write
		if command[0] == "1":	
			# digital
			if command[1] == "1":
				pin = command[2]
				freq = command[3]
				print("udp2uart : digital write pin "+pin+" "+freq+"")
			# analog		
			elif command[1] == "2":
				pin = command[2]
				if command[3] == "0":
					pinstate = "LOW"
				elif command[3] == "1":
					pinstate = "HIGH"
				print("udp2uart : analog write pin "+pin+" "+pinstate+"")
		# pin read
		elif command[0] == "2":
			# digital
			if command[1] == "1":
				pin = command[2]
				print("udp2uart : digital read pin "+pin+"")
			# analog
			elif command[1] == "2":
				pin = command[2]
				print("udp2uart : analog read pin "+pin+"")
		# servo
		elif command[0] == "3":
			# read
			if command[1] == "1":
				pin = command[2]
				print("udp2uart : servo read pin "+pin+"")
			# write
			elif command[1] == "2":
				pin = command[2]
				pos = command[3]
				print("udp2uart : servo write pin "+pin+" pos "+pos+"")
		# shift register
		elif command[0] == "4":
			dataPin = command[1]
			latchPin = command[2]
			clockPin = command[3]
			SRegdata = command[4]
			print("udp2uart : shift register write "+SRegdata+" (data : "+dataPin+", latch : "+latchPin+", clock : "+clockPin+"")
		# PING sensor
		elif command[0] == "6":
			trigPin = command[1]
			echoPin = command[2]
			print("udp2uart : PING sensor read (trigger : "+trigPin+", echo : "+echoPin+")")
		# PWM
		elif command[0] == "7":
			pwmPin = command[1]
			pwmVar = command[2]
			print("udp2uart : PWM write (pin : "+pwmPin+", var : "+pwmVar+")")
	if arduino != "":
		try: 
			arduino.write(data)
			if command[0] == "2":
				# sleep(0.01)
				read_data=arduino.readline().strip('\r\n').strip()
				if read_data:
					if read_data == "nan":
						sensor_value = "data error"
					else:
						sensor_value = read_data
						if(UDPSock.sendto(sensor_value,addr)):	
							print "udp2uart : Renvoi de "+str(sensor_value)+" vers "+str(addr)+""		
				else:
					sensor_value = "read error"
			elif command[0] == "6":
				# sleep(0.01)
				read_data=arduino.readline().strip('\r\n').strip()
				if read_data:
					if read_data == "nan":
						sensor_value = "data error"
					else:
						sensor_value_cm = trunc((float(read_data) / 58), 2)
						if(UDPSock.sendto(read_data,addr)):
							print "udp2uart : Renvoi de "+read_data+" vers "+str(addr)+""
			#else:
				#if(UDPSock.sendto("OK",addr)):
					#print "Renvoi de : OK vers "+str(addr)+" ... "
		except:
			print "udp2uart : erreur lors de l envoi de la commande"

os.system("mkdir "+write_dir+"")
while(found_device == ""):
	get_uart_device()
	if found_device == "":
		sleep(5)
# get_uart_device()

# --- Ecoute socket UDP ---
# On fait le socket
# et bind l'addresse
UDPSock = socket(AF_INET,SOCK_DGRAM)
UDPSock.bind(addr)

while 1:
    data,addr = UDPSock.recvfrom(buf)
    if not data:
        print "Le client a quitte"
        break
    else:
	#print "Recu: "+data+" de "+str(addr)+""
	uart_dialog(data,addr)

# On ferme tout ca
UDPSock.close()
# --- FIN Ecoute socket UDP ---

client_udp.py

# Le client
# On importe socket..
from socket import *
import uinput
import xbox_read

# Variables
host = "localhost"
port = 2074
buf = 1024
addr = (host,port)

i = 0

# On fait le socket
UDPSock = socket(AF_INET,SOCK_DGRAM)
def_msg = "Message?"
print "\n",def_msg

# events = pygame.event.get()
max_val = 32768

# Envois du message
while (1):

	for event in xbox_read.event_stream(deadzone=12000):
		if event.key == "Y1":
			print("left")
			motor = "5"
			value = event.value/128
			print(value)
			data = "7/"+str(motor)+"/"+str(value)+"/"
			print(data)
			if value >= 0:
				if(UDPSock.sendto(str(data),addr)):
					print "("+str(i)+") Envoi de: '",data,"' ... "
		if event.key == "Y2":
			print("right")
			motor = "3"
			value = event.value/128
			print(value)
			data = "7/"+str(motor)+"/"+str(value)+"/"
			if value >= 0:
				if(UDPSock.sendto(str(data),addr)):
					print "("+str(i)+") Envoi de: '",data,"' ... "

About Captain Stouf

Spécialiste en systèmes informatiques, Développeur matériel et logiciel, Inventeur, Maker : électronique, Systems on Chip, micro-controlleurs, Internet of Things, Modélisation / Scan / Impression 3D, Imagerie...

One thought on “Makers – Robot Hack – Jour 2

Laisser un commentaire