Réaliser une télécommande avec un Raspberry Pi et un Smartphone Android
Je vais expliquer sur ce post comment utiliser un Raspberry Pi et un Smartphone comme Radio Commande à la manière de l'AR Drone.
J'ai modifié une antique Tamiya Grasshopper des années 90 afin de réaliser ce projet. Le but est d'utiliser un minimum de matériel afin de réduire les couts au minimum. On se passera donc de carte d'extension type Adafruit trop couteuse.
Le seul materiel requis ici sera :
- un Raspberry Pi model B + une carte SD ( 2GB minimum )
- 2 servomoteurs ( on peut réutiliser les servos d'origine du véhicule ).
- Un regulateur de tension afin d'alimenter le Raspberry avec le Pack d'Accu du véhicule.
- une clé USB WiFi
- une camera pour Raspberry comme ci dessous. Vous pouvez utiliser egalement une camera USB supportant nativement le M-JPEG.
Réalisation
Pour commencer, installer une Raspbian sur votre Raspberry. Les tutos à ce sujet sont nombreux sur le net ( par exemple ce lien ).
Lorsque votre plateforme est prête, vous pouvez booter votre Raspberry.
Alimentation de la carte
Afin d'éviter un bloc batterie supplémentaire, le plus simple est d'utiliser directement le bloc d'accus de la voiture. Celui-ci fournit du 7,2 Volt qu'il va falloir abaisser à 5 Volts pour alimenter le raspberry et les servos.Le plus simple est d'utiliser un regulateur de tension. J'ai préféré ne pas utiliser le traditionnel LM7805 dont le rendement est assez mauvais pour un convertisseur DC-DC plus souple consommant moins de courant. J'ai choisi le TSR 1-2450 de TRACOPOWER qui délivre un courant stable ne risquant pas de planter le Raspberry.
Création de la commande des servos :
Un servomoteur utilise 3 fils pour fonctionner
- une alimentation +5V
- une masse GND
- un fil de commande SIG+
Voici un exemple des cablages repandu chez differents constructeurs :
le servo est alimenté entre le +5 Volts et la masse GND ( Attention, ne pas utiliser la sortie +5V du Raspberry qui ne peut pas fournir assez d'énergie pour cela / Utiliser une alimentation externe. Nous verrons par la suite comment utiliser directement le pack d'accus pour cela ).
Pour faire bouger le servo dans une position voulue, nous allons générer un signal carré d'une periode de 20ms et de tension 5Volt sur le fil SIG
Le schéma ci dessous montre comment positionner le servo en fonction du signal injecté sur SIG
le signal SIG sera généré automatiquement par les GPIOs du Raspberry afin d'éviter l'achat d'une carte addtionnelle. La masse GND du servos sera raccordée à la masse du Raspberry ( PIN 6 du connecteur GPIO )
- Servo 0 : GPIO 4 ( PIN 7 ) : Commande de direction
- Servo 1 : GPIO 17 ( PIN 11 ) : Commande d'accelerateur
Schéma de câblage ci dessous :
Pour générer ce signal et piloter les servos, j'utilise l'excellent driver "Servoblaster". Attention cependant, l'utilisation des GPIOs directement sur les servomoteurs sont à vos risques et perils. Il convient de s'assurer que les servos ne consomment pas plus de 15mA sur l'entrée SIG, ce qui devrait etre le cas de la plupart des modeles s'ils sont raccordés correctement.
Décompresser le fichier, copier le dans /home/pi, renommer le fichier décompressé de "PiBits-master" en "PiBits".
Puis pour l'installer :
pi@raspberrypi ~ $ cd PiBits/ServoBlaster/user
pi@raspberrypi ~/PiBits/ServoBlaster/user $ make servod
pi@raspberrypi ~/PiBits/ServoBlaster/user $ make servod
Pour tester le bon fonctionnement :
Brancher un servo sur le raspberry :
Mettre une alim de 5 volts ( ou 4.5 ) entre GND et +5V sur le servo
Raccorder SIG à la PIN 7 du Raspberry et GND a la PIN 6
Lancer le driver sur 2 servomoteurs :
pi@raspberrypi ~/PiBits/ServoBlaster/user $ sudo ./servod --p1pins=7,11
Puis taper la commande :
pi@raspberrypi ~/PiBits/ServoBlaster/user $ echo 0=150 > /dev/servoblaster
Les valeurs vont en général de 70 a 220 en fonction des servos utilisés.
Pour ajouter d'autres servos ( on peut en contrôler jusqu'à 8 ), le mapping de servo blaster est le suivant :
Les valeurs vont en général de 70 a 220 en fonction des servos utilisés.
Pour ajouter d'autres servos ( on peut en contrôler jusqu'à 8 ), le mapping de servo blaster est le suivant :
Servo number GPIO number P1 header 0 4 P1-7 1 17 P1-11 2 18 P1-12 3 21 P1-13 4 22 P1-15 5 23 P1-16 6 24 P1-18
7 25 P1-22
Le P1 header indique la position de la broche sur la Raspberry Pi. Ci-dessous, le schema du connecteur du Raspberry Pi B
Réalisation de la télécommande
Pour réaliser la télécommande, on va établir un petit protocol d'échanges entre le smartphone et le Raspberry. Comme nous sommes à l'extérieur, et qu'il n'y a pas de point d'accès WiFi accessible, on va connecter directement le smartphone au Raspberry en WiFi
La premiere étape consiste à connecter une clé WiFi USB au Raspberry.
J'ai utilisé une clé Bewan mais celle ci n'est plus en vente aujourd'hui. On peut utiliser n'importe quelle clé WiFi reconnue par le Raspberry Pi ayant un driver sur Raspbian telle la TP Link TL-WN725N
L'important est que la clé WiFi soit compatible avec hostapd
Pour faciliter la connection du Smartphone, on va configurer le Raspberry en point d'accès WiFi.
Pour cela, le plus simple est d'installer "hostapd" et "isc-dhcp-server". Les tutos expliquant comment configurer son raspberry en point d'accès WiFi sont nombreux. Vous pouvez suivre celui-ci par exemple :
Le nom du point d'accès Wifi et sa configuration sont à votre choix et sans importance pour la suite. De mon coté, j'ai configuré de la manière suivante :
Point d'accès : GRASSHOPPER
Mot de passe 123456
IP : 10.10.0.1
hostapd.conf
interface=wlan0
##driver=rt2870sta
ssid=GRASSHOPPER
hw_mode=g
channel=4
wpa=2
wpa_passphrase=123456
## Key management algorithms ##
wpa_key_mgmt=WPA-PSK
## Set cipher suites (encryption algorithms) ##
## TKIP = Temporal Key Integrity Protocol
## CCMP = AES in Counter mode with CBC-MAC
wpa_pairwise=TKIP
rsn_pairwise=CCMP
## Shared Key Authentication ##
auth_algs=1
## Accept all MAC address ###
macaddr_acl=0
/etc/network/interfaces
auto lo
iface lo inet loopback
iface eth0 inet dhcp
#allow-hotplug wlan0
iface wlan0 inet static
address 10.10.0.1
netmask 255.255.255.0
#wpa-roam /etc/wpa_supplicant/wpa_supplicant.conf
#iface default inet dhcp
plutôt que dnsmasq, j'ai utilisé dhcpd
sudo apt-get install isc-dhcp-server
subnet 10.10.0.0 netmask 255.255.255.0 {
range 10.10.0.25 10.10.0.50;
interface wlan0;
}
A partir de maintenant, vous devez pouvoir connecter votre téléphone mobile ou un PC au Raspberry Pi en WiFi. Le pc ou le mobile recevront une adresse IP automatiquement.
On va maintenant pouvoir passer à la télécommande elle même. elle est basée sur l'envoi en UDP d'une trame texte comme ci dessous toutes les 20 millisecondes :
X:Y:Offset_X:Offset_Y
Ou X et Y sont la position de la commande accélérateur et direction
Offset_X et Offset_Y permettent d'ajuster le centrage de la télécommande
Coté Raspberry, on va faire un petit programme en C compilable avec gcc.
Servod.c
#include <stdio.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#define MAX_SIZE 50
#define PORT 5001
#define Neutral_X 150
#define Neutral_Y 150
#define minX 90
#define minY 90
#define maxX 210
#define maxY 210
int main()
{
// Two socket descriptors which are just integer numbers used to access a socket
int sock_descriptor, conn_desc;
int connected=0;
// Two socket address structures - One for the server itself and the other for client
struct sockaddr_in serv_addr, client_addr;
// Buffer to store data read from client
char buff[MAX_SIZE];
// Create socket of domain - Internet (IP) address, type - Stream based (TCP) and protocol unspecified
// since it is only useful when underlying stack allows more than one protocol and we are choosing one.
// 0 means choose the default protocol.
sock_descriptor = socket(AF_INET, SOCK_STREAM, 0);
// A valid descriptor is always a positive value
if(sock_descriptor < 0)
printf("Failed creating socket\n");
// Initialize the server address struct to zero
bzero((char *)&serv_addr, sizeof(serv_addr));
// Fill server's address family
serv_addr.sin_family = AF_INET;
// Server should allow connections from any ip address
serv_addr.sin_addr.s_addr = INADDR_ANY;
// 16 bit port number on which server listens
// The function htons (host to network short) ensures that an integer is interpretted
// correctly (whether little endian or big endian) even if client and server have different architectures
serv_addr.sin_port = htons(PORT);
// Attach the server socket to a port. This is required only for server since we enforce
// that it does not select a port randomly on it's own, rather it uses the port specified
// in serv_addr struct.
if (bind(sock_descriptor, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
printf("Failed to bind\n");
// Server should start listening - This enables the program to halt on accept call (coming next)
// and wait until a client connects. Also it specifies the size of pending connection requests queue
// i.e. in this case it is 5 which means 5 clients connection requests will be held pending while
// the server is already processing another connection request.
listen(sock_descriptor, 1);
printf("Waiting for connection...\n");
int size = sizeof(client_addr);
while(1)
{
// Server blocks on this call until a client tries to establish connection.
// When a connection is established, it returns a 'connected socket descriptor' different
// from the one created earlier.
conn_desc = accept(sock_descriptor, (struct sockaddr *)&client_addr, &size);
if (conn_desc == -1)
printf("Failed accepting connection\n");
else
{
printf("Connected\n");
connected=1;
}
while(connected)
{
// The new descriptor can be simply read from / written up just like a normal file descriptor
if ( read(conn_desc, buff, sizeof(buff)-1) > 0)
{
char command[256];
char* numbuff;
int X,Y,sX,sY,oX,oY;
//printf("Received %s\n", buff);
strcpy(command, strtok(buff,":\n"));
X=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
Y=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
oX=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
oY=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
printf("message %s / X=%d / Y=%d\n",command,X,Y);
// SEtup the servos position
FILE * ServoF;
if( (ServoF = fopen("/dev/servoblaster","w")) != NULL ) {
fprintf(ServoF,"0=%d\n",sX=(int)(Neutral_X+oX+(X*(maxX-Neutral_X)/10)));
fprintf(ServoF,"1=%d\n",sY=(int)(Neutral_Y+oY+(Y*(maxY-Neutral_Y)/10)));
printf("Servos : X=%d / Y=%d\n",sX,sY);
}
fclose(ServoF);
}
else
{
printf("Failed receiving\n");
connected=0;
}
}
// Program should always close all sockets (the connected one as well as the listening one)
// as soon as it is done processing with it
close(conn_desc);
}
close(sock_descriptor);
return 0;
}
#include <unistd.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#define MAX_SIZE 50
#define PORT 5001
#define Neutral_X 150
#define Neutral_Y 150
#define minX 90
#define minY 90
#define maxX 210
#define maxY 210
int main()
{
// Two socket descriptors which are just integer numbers used to access a socket
int sock_descriptor, conn_desc;
int connected=0;
// Two socket address structures - One for the server itself and the other for client
struct sockaddr_in serv_addr, client_addr;
// Buffer to store data read from client
char buff[MAX_SIZE];
// Create socket of domain - Internet (IP) address, type - Stream based (TCP) and protocol unspecified
// since it is only useful when underlying stack allows more than one protocol and we are choosing one.
// 0 means choose the default protocol.
sock_descriptor = socket(AF_INET, SOCK_STREAM, 0);
// A valid descriptor is always a positive value
if(sock_descriptor < 0)
printf("Failed creating socket\n");
// Initialize the server address struct to zero
bzero((char *)&serv_addr, sizeof(serv_addr));
// Fill server's address family
serv_addr.sin_family = AF_INET;
// Server should allow connections from any ip address
serv_addr.sin_addr.s_addr = INADDR_ANY;
// 16 bit port number on which server listens
// The function htons (host to network short) ensures that an integer is interpretted
// correctly (whether little endian or big endian) even if client and server have different architectures
serv_addr.sin_port = htons(PORT);
// Attach the server socket to a port. This is required only for server since we enforce
// that it does not select a port randomly on it's own, rather it uses the port specified
// in serv_addr struct.
if (bind(sock_descriptor, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
printf("Failed to bind\n");
// Server should start listening - This enables the program to halt on accept call (coming next)
// and wait until a client connects. Also it specifies the size of pending connection requests queue
// i.e. in this case it is 5 which means 5 clients connection requests will be held pending while
// the server is already processing another connection request.
listen(sock_descriptor, 1);
printf("Waiting for connection...\n");
int size = sizeof(client_addr);
while(1)
{
// Server blocks on this call until a client tries to establish connection.
// When a connection is established, it returns a 'connected socket descriptor' different
// from the one created earlier.
conn_desc = accept(sock_descriptor, (struct sockaddr *)&client_addr, &size);
if (conn_desc == -1)
printf("Failed accepting connection\n");
else
{
printf("Connected\n");
connected=1;
}
while(connected)
{
// The new descriptor can be simply read from / written up just like a normal file descriptor
if ( read(conn_desc, buff, sizeof(buff)-1) > 0)
{
char command[256];
char* numbuff;
int X,Y,sX,sY,oX,oY;
//printf("Received %s\n", buff);
strcpy(command, strtok(buff,":\n"));
X=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
Y=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
oX=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
oY=((numbuff = strtok(NULL,":\n")) != NULL?atoi(numbuff):0);
printf("message %s / X=%d / Y=%d\n",command,X,Y);
// SEtup the servos position
FILE * ServoF;
if( (ServoF = fopen("/dev/servoblaster","w")) != NULL ) {
fprintf(ServoF,"0=%d\n",sX=(int)(Neutral_X+oX+(X*(maxX-Neutral_X)/10)));
fprintf(ServoF,"1=%d\n",sY=(int)(Neutral_Y+oY+(Y*(maxY-Neutral_Y)/10)));
printf("Servos : X=%d / Y=%d\n",sX,sY);
}
fclose(ServoF);
}
else
{
printf("Failed receiving\n");
connected=0;
}
}
// Program should always close all sockets (the connected one as well as the listening one)
// as soon as it is done processing with it
close(conn_desc);
}
close(sock_descriptor);
return 0;
}
on peut le compiler simplement sur le Raspberry avec :
gcc Servod.c -o servod
Ce programme sera lancé au boot du Rasberry en lancant : ./servod
Pour le retour video, on va utiliser gstreamer1.0. Pour l'installation, il y a pas mal de autos dont celui-ci :
http://pi.gbaman.info/?p=150
On peut le lancer avec la commande suivante :
gst-launch-1.0 -v v4l2src ! video/x-raw,width=320,height=240,framerate=\(fraction\)24/1 ! queue ! videorate ! video/x-raw,framerate=10/1 ! jpegenc ! multipartmux ! tcpserversink host=10.10.0.1 port=5000
Application mobile
Pour manipuler la voiture et recevoir le flux video de la camera, j'ai fait une petite application android. Vous pouvez la télécharger sur ce lien :
https://www.dropbox.com/s/rphclxuk0n7qdvq/Telecommande.apk?dl=0
