[PiRobot] Partie 4 – Refactoring du code PiRobot et réflexions sur le pilotage

Dans cette partie, on va coder un peu, faire un code propre et présentable du côté PiRobot. Je vais également aborder la mise en forme de mes trames UDP ainsi que la parallélisassions des tâches sur le PiRobot avec l’utilisation du multithreading. Après quoi, on aura un PiRobot entièrement pilotable via le réseau Wifi ! Je veux également faire un point sur l’avancement du client sur tablette Windows qui avance très doucement pour le moment, d’où un point de réflexion sur comment le PiRobot doit être piloté. Quelles stratégies sont à mettre en place pour que PiRobot obéisse au doigt et à l’œil ?

Table des matières

1. Petit refactoring de code côté serveur

Je vais reprendre la définition d’introduction du Wiki sur le « code refactoring » ou le « réusinage de code » :

Le réusinage de code est l’opération consistant à retravailler le code source d’un programme informatique – sans toutefois y ajouter des fonctionnalités ni en corriger les bogues – de façon à en améliorer la lisibilité et par voie de conséquence la maintenance, ou à le rendre plus générique (afin par exemple de faciliter le passage de simple en multiple précision) ; on parle aussi de « remaniement ». Cette technique utilise quelques méthodes propres à l’optimisation de code, avec des objectifs différents.
Le terme réusinage est originaire du Québec. L’équivalent en anglais est code refactoring, parfois rendu par refactorisation, terme qui, selon l’OQLF (ndlr: Office québécois de la langue française), est à éviter.

Fin de l’instant culture 🙂

1.1. Une fonction unique pour piloter les moteurs

Pour piloter un moteur comme j’en ai envie, j’ai actuellement besoin de trois variables :

Son n° de moteur (qui redirige sur son adresse PWM)
Sa vitesse
Le sens de rotation

Cela fait trois information à traiter par moteur, soit 3*4 = 12 informations à traiter. C’est-à-dire que notre trame qui circulera entre notre console de pilotage et le PiRobot devra contenir au minimum 12 informations qu’il faudra séparer et décoder. Or plus la trame est simple, plus il sera facile et rapide de la décoder ! C’est du gain de temps à la fois pour le développeur (#moi) et pour la machine.

Afin de réduire ce nombre d’info, je souhaitai réaliser une fonction unique permettant de directement piloter chaque moteur sans s’embêter à en changer le sens, mais en ajoutant un signe à la vitesse demandée :

Fonction

En faisant cela je réduis le nombre d’information de 12 à 4, car je me repère ainsi :

Le signe représentante mon sens de rotation par rapport à l’orientation du PiRobot (caméra à l’avant)
La position de l’information dans la trame représente le n° du moteur à piloter
Le nombre représente toujours ma vitesse
- 0 pour 0% de la puissance maximale du moteur
- 4096 pour 100% de la puissance maximale du moteur

Il faut également que je prenne en compte de la commande du sens de rotation des moteurs est inversé dans mon cas pour les moteurs arrière et avant. Plutôt que de le corriger de manière hardware, je corrige ça directement dans le code :

Fonction de commande des moteurs

//-----------------------------
//----- Prototypes utiles -----
//-----------------------------

int cmdMoteur(int moteur, signed int vitesse);


//------------------------------
//------ Fonctions utiles ------
//------------------------------

/// <summary>  
///  Retour : int à 0 par défaut.
///  Paramètres :
///		moteur : 1 à 4 pour commander les différents moteurs
///		vitesse : Vitesse définie entre -4096 (arrière) et 4096 (avant) pour chaque moteur
/// </summary>  
int cmdMoteur(int moteur, signed int vitesse)
{
	if (abs(vitesse) <= 4096) 
	{
		switch (moteur)
		{
		case 1:
			if (vitesse < 0) 
			{
				digitalWrite(SENS_MOTEUR_1, LOW);	//recule
				pwmWrite(PWM_MOTEUR_1, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_1, HIGH);	//avance
				pwmWrite(PWM_MOTEUR_1, vitesse);
			}
			break;
		case 2:
			if (vitesse < 0)
			{
				digitalWrite(SENS_MOTEUR_2, LOW);	//recule
				pwmWrite(PWM_MOTEUR_2, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_2, HIGH);	//avance
				pwmWrite(PWM_MOTEUR_2, vitesse);
			}
			break;
		case 3:
			if (vitesse < 0)
			{
				digitalWrite(SENS_MOTEUR_3, HIGH);	//recule
				pwmWrite(PWM_MOTEUR_3, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_3, LOW);	//avance
				pwmWrite(PWM_MOTEUR_3, vitesse);
			}
			break;
		case 4:
			if (vitesse < 0)
			{
				digitalWrite(SENS_MOTEUR_4, HIGH);	//recule
				pwmWrite(PWM_MOTEUR_4, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_4, LOW);	//avance
				pwmWrite(PWM_MOTEUR_4, vitesse);
			}
			break;

		default:
			return 1; //code erreur 1 : mauvais moteur
			break;
		}
	}
	else 
	{
		return 2; //code erreur 2 : out of limit
	}
	return 0;
}

Ensuite je crée une fonction qui sera notre routine et qui balaiera chaque moteur en revue à tout instant. Nous récupérons les informations moteurs de la variable globale cons_pwm[5] (explications au prochain chapitre !).

Routine de contrôle des moteurs

//----------------------------
//---- Variables globales ----
//----------------------------
signed int cons_pwm[5];

//-----------------------------
//----- Prototypes utiles -----
//-----------------------------

void ctrlPiRobot_thread();

//------------------------------
//------ Fonctions utiles ------
//------------------------------

void ctrlPiRobot_thread() 
{
	while (true)
	{
		cmdMoteur(1, cons_pwm[0]);
		cmdMoteur(2, cons_pwm[1]);
		cmdMoteur(3, cons_pwm[2]);
		cmdMoteur(4, cons_pwm[3]);
	}
}

1.2. Mise en forme et décodage de la trame UDP

La trame que la tablette enverra aura cette forme :

mot1;mot2;mot3;mot4;servomot1

Le séparateur entre les différentes informations sera le « point-virgule ».

Trame UDP reçue

En vert : déjà implémenté et testé dans le code.
En jaune : implémenté dans le code, reste à tester.

Afin de décoder les trames entrantes et donc de séparer les données grâce à l’information point-virgule, j’utilise le code suivant :

Décodage de la trame grâce au séparateur

//Ici, on met en forme le tableau de int à partir du tableau de char "buf" 
//chaque mot contient max 5 caractères + le caract. de fin de char = \0
//Le séparateur interprété est le ';'
//La chaîne reçue est de type "+0000;-0000;+0000;-0000;+0000" ou "0;0;0;0;0"
//Pour "pwm1;pwm2;pwm3;pwm4;pwm5"
//Chaque "pwm" représente un "mot"

		char mot[6];
        
		int i = 0, j = 0, state = 1, num = 0;
		
		while (buf[i] != '\0') {
			if (state == 1)
			{
				if (buf[i] != ';')
				{
					state = 2; j = 0; mot[0] = buf[i]; j++;
				}
			}
			else
			{
				if (buf[i] != ';') { mot[j] = buf[i]; j++; }
				else
				{
					state = 1;
					mot[j] = '\0';
					cons_pwm[num] = atoi(mot);
					num++;
				}
			}
			i++;
		}
		if (state == 2)
		{
			mot[j] = '\0';
			cons_pwm[num] = atoi(mot);
			num++;
		}

Ce code permet de récupérer chaque information dans un tableau contenant 5 Int32 signés : cons_pwm[5].
Le serveur UDP renverra ces informations-là :

Trame de retour UDP

Orange : Reste à implémenter et à tester.
Orange foncé : Reste à étudier, fera l’objet d’un article séparé.

Actuellement, cette trame est représentée par la variable string : « Etat PiRobot ». Je m’occuperais du retour de trame lorsque j’aurai une bibliothèque fonctionnelle pour obtenir l’information analogique du courant sur les moteurs.

Voici le code complet de la partie serveur UDP :

Code de la partie serveur UDP

//-----------------------------
//----- Prototypes utiles -----
//-----------------------------
void udpServer_thread();
void error(const char *msg);

//----------------------------
//---- Variables globales ----
//----------------------------
signed int cons_pwm[5];


//------------------------------
//------ Fonctions utiles ------
//------------------------------
void error(const char *msg)
{
	perror(msg);
	exit(0);
}


void udpServer_thread() {
	std::cout << "Lancement du serveur UDP..." << std::endl;

	int sock, length, n;
	socklen_t fromlen;
	struct sockaddr_in server;
	struct sockaddr_in from;
	char buf[1024];

	sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (sock < 0) error("Opening socket");
	length = sizeof(server);
	bzero(&server, length);
	server.sin_family = AF_INET;
	server.sin_addr.s_addr = INADDR_ANY;
	server.sin_port = htons(26000);	//Ouverture de la socket sur le port 26000
	if (bind(sock, (struct sockaddr *)&server, length) < 0)
		error("binding");
	fromlen = sizeof(struct sockaddr_in);
	std::cout << "Serveur UDP en écoute sur le port 26000 !" << std::endl;
	while (1) {
		n = recvfrom(sock, buf, 1024, 0, (struct sockaddr *)&from, &fromlen);
		if (n < 0) error("recvfrom");
		//write(1, "Client : ", 9);
		write(1, buf, n);
		write(1, "\n", 1);
		n = sendto(sock, "Etat PiRobot", 12,
			0, (struct sockaddr *)&from, fromlen);
		if (n < 0) error("sendto");


		//Ici, on met en forme le tableau de int à partir du tableau de char "buf" 
		//chaque mot contient max 5 caractères + le caract. de fin de char = \0
		//Le séparateur interprété est le ';'
		//La chaîne reçue est de type "+0000;-0000;+0000;-0000;+0000" ou "0;0;0;0;0"
		//Pour "pwm1;pwm2;pwm3;pwm4;pwm5"
		char mot[6];
        
		int i = 0, j = 0, state = 1, num = 0;
		
		while (buf[i] != '\0') {
			if (state == 1)
			{
				if (buf[i] != ';')
				{
					state = 2; j = 0; mot[0] = buf[i]; j++;
				}
			}
			else
			{
				if (buf[i] != ';') { mot[j] = buf[i]; j++; }
				else
				{
					state = 1;
					mot[j] = '\0';
					cons_pwm[num] = atoi(mot);
					num++;
				}
			}
			i++;
		}
		if (state == 2)
		{
			mot[j] = '\0';
			cons_pwm[num] = atoi(mot);
			num++;
		}

	}

}

1.3. Parallélismes des tâches Serveur UDP et Commande PiRobot grâce au Multithread

Maintenant que nous avons nos deux parties de code distincts (serveur UDP et commande PiRobot), voyons comment on peut simplement faire tourner les deux morceaux de codes ensemble. Pour ce faire, il suffit d’appeler les fonctions crées en threads. Le serveur UDP décode les informations des trames entrantes et les stockent dans un tableau de Int32 qui nous sert de variable globale. C’est cette variable qui nous permet de transiter les informations d’un thread à l’autre.

Variable partagée entre deux threads

On crée les threads dans la fonction « main » et on les appelles jusqu’à la fin de leur exécution (ils n’ont théoriquement pas de fin, car les deux threads contiennent des boucles while infinie : une pour l’attente de message UDP, et l’autre pour la routine de contrôle du Robot).

Fonction Main du PiRobot

int main(void)
{
	wiringPiSetupSys(); //initialise WiringPi

	//initialisation des modes sur les différents PIN
	pinMode(SENS_MOTEUR_1, OUTPUT);
	pinMode(SENS_MOTEUR_2, OUTPUT);
	pinMode(SENS_MOTEUR_3, OUTPUT);
	pinMode(SENS_MOTEUR_3, OUTPUT);

	//initialisation du PCA9685
	int fd = pca9685Setup(PIN_BASE, 0x40, HERTZ);
	if (fd < 0)
	{
		printf("Erreur dans l'initialisation PWM\n");
		return fd;
	}
	pca9685PWMReset(fd);
	printf("La fréquence PWM est définie sur %d hertz\n", HERTZ);

	std::thread t1(udpServer_thread);
	std::thread t2(ctrlPiRobot_thread);
    t1.join();
	t2.join();

	
		return 0;
	}

A partir de là, il est déjà possible de piloter le PiRobot avec de simples trames UDP (via Packet Sender par exemple).

L’intégralité du code ci-dessous, à la date du 30/01/2017 :

Code PiRobot au 30/01/2017

#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "pca9685.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <sys/types.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <netdb.h>

#include <iostream>
#include <thread>


//-----------------------------
//----- Prototypes utiles -----
//-----------------------------

int cmdMoteur(int moteur, signed int vitesse);
void error(const char *msg);
void udpServer_thread();
void ctrlPiRobot_thread();


//----------------------------
//---- Variables globales ----
//----------------------------
signed int cons_pwm[5];

//-----------------
//---- DEFINES ----
//-----------------

#define PIN_BASE 300

#define PWM_MOTEUR_1 300
#define	SENS_MOTEUR_1 4

#define PWM_MOTEUR_2 301
#define	SENS_MOTEUR_2 17

#define PWM_MOTEUR_3 302
#define	SENS_MOTEUR_3 27

#define PWM_MOTEUR_4 303
#define	SENS_MOTEUR_4 22

#define PWM_SERVO_CAMERA 304

#define MAX_PWM 4096
#define HERTZ 50


int main(void)
{
	wiringPiSetupSys(); //initialise WiringPi

	//initialisation des modes sur les différents PIN
	pinMode(SENS_MOTEUR_1, OUTPUT);
	pinMode(SENS_MOTEUR_2, OUTPUT);
	pinMode(SENS_MOTEUR_3, OUTPUT);
	pinMode(SENS_MOTEUR_3, OUTPUT);

	//initialisation du PCA9685
	int fd = pca9685Setup(PIN_BASE, 0x40, HERTZ);
	if (fd < 0)
	{
		printf("Erreur dans l'initialisation PWM\n");
		return fd;
	}
	pca9685PWMReset(fd);
	printf("La fréquence PWM est définie sur %d hertz\n", HERTZ);

	std::thread t1(udpServer_thread);
	std::thread t2(ctrlPiRobot_thread);
    t1.join();
	t2.join();

	
		return 0;
}


//------------------------------
//------ Fonctions utiles ------
//------------------------------

/// <summary>  
///  Retour : int à 0 par défaut.
///  Paramètres :
///		moteur : 1 à 4 pour commander les différents moteurs
///		vitesse : Vitesse définie entre -4096 (arrière) et 4096 (avant) pour chaque moteur
/// </summary>  
int cmdMoteur(int moteur, signed int vitesse)
{
	if (abs(vitesse) <= 4096) 
	{
		switch (moteur)
		{
		case 1:
			if (vitesse < 0) 
			{
				digitalWrite(SENS_MOTEUR_1, LOW);	//recule
				pwmWrite(PWM_MOTEUR_1, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_1, HIGH);	//avance
				pwmWrite(PWM_MOTEUR_1, vitesse);
			}
			break;
		case 2:
			if (vitesse < 0)
			{
				digitalWrite(SENS_MOTEUR_2, LOW);	//recule
				pwmWrite(PWM_MOTEUR_2, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_2, HIGH);	//avance
				pwmWrite(PWM_MOTEUR_2, vitesse);
			}
			break;
		case 3:
			if (vitesse < 0)
			{
				digitalWrite(SENS_MOTEUR_3, HIGH);	//recule
				pwmWrite(PWM_MOTEUR_3, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_3, LOW);	//avance
				pwmWrite(PWM_MOTEUR_3, vitesse);
			}
			break;
		case 4:
			if (vitesse < 0)
			{
				digitalWrite(SENS_MOTEUR_4, HIGH);	//recule
				pwmWrite(PWM_MOTEUR_4, abs(vitesse));
			}
			else
			{
				digitalWrite(SENS_MOTEUR_4, LOW);	//avance
				pwmWrite(PWM_MOTEUR_4, vitesse);
			}
			break;

		default:
			return 1; //code erreur 1 : mauvais moteur
			break;
		}
	}
	else 
	{
		return 2; //code erreur 2 : out of limit
	}
	return 0;
}

void error(const char *msg)
{
	perror(msg);
	exit(0);
}

void udpServer_thread() 
{
	std::cout << "Lancement du serveur UDP..." << std::endl;

	int sock, length, n;
	socklen_t fromlen;
	struct sockaddr_in server;
	struct sockaddr_in from;
	char buf[1024];

	sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (sock < 0) error("Opening socket");
	length = sizeof(server);
	bzero(&server, length);
	server.sin_family = AF_INET;
	server.sin_addr.s_addr = INADDR_ANY;
	server.sin_port = htons(26000);
	if (bind(sock, (struct sockaddr *)&server, length) < 0)
		error("binding");
	fromlen = sizeof(struct sockaddr_in);
	std::cout << "Serveur UDP en écoute sur le port 26000 !" << std::endl;
	while (1) {
		n = recvfrom(sock, buf, 1024, 0, (struct sockaddr *)&from, &fromlen);
		if (n < 0) error("recvfrom");
		//write(1, "Client : ", 9);
		write(1, buf, n);
		write(1, "\n", 1);
		n = sendto(sock, "Etat PiRobot", 12,
			0, (struct sockaddr *)&from, fromlen);
		if (n < 0) error("sendto");


		//Ici, on met en forme le tableau de int à partir du tableau de char "buf" 
		//chaque mot contient max 5 caractères + le caract. de fin de char = \0
		//Le séparateur interprété est le ';'
		//La chaîne reçue est de type "+0000;-0000;+0000;-0000;+0000" ou "0;0;0;0;0"
		//Pour "pwm1;pwm2;pwm3;pwm4;pwm5"
		char mot[6];
        
		int i = 0, j = 0, state = 1, num = 0;
		
		while (buf[i] != '\0') 
		{
			if (state == 1)
			{
				if (buf[i] != ';')
				{
					state = 2; j = 0; mot[0] = buf[i]; j++;
				}
			}
			else
			{
				if (buf[i] != ';') { mot[j] = buf[i]; j++; }
				else
				{
					state = 1;
					mot[j] = '\0';
					cons_pwm[num] = atoi(mot);
					num++;
				}
			}
			i++;
		}
		if (state == 2)
		{
			mot[j] = '\0';
			cons_pwm[num] = atoi(mot);
			num++;
		}

	}

}

void ctrlPiRobot_thread() 
{
	while (true)
	{
		cmdMoteur(1, cons_pwm[0]);
		cmdMoteur(2, cons_pwm[1]);
		cmdMoteur(3, cons_pwm[2]);
		cmdMoteur(4, cons_pwm[3]);
	}
}

2. Arrêt sur image : Interface client et check-list PiRobot serveur

En parallèle de la programmation sur le Raspberry Pi, j’ai un peu avancé sur l’interface client (un peu… hein ^^).

J’aimerai procéder par divers onglets :

Connexion : affiche les informations de connexions au PiRobot et permet d’accéder aux informations brutes avec un afficheur texte qui permettrait de faire du Debug (contrôler les échanges de trames par exemple…).
Pilotage : Ce sera mon interface de pilotage du PiRobot, avec un retour vidéo du PiRobot, des boutons pour avancer/reculer et contrôler la direction. Il faudra prévoir un contrôle de l’orientation de la caméra se trouvant sur le servomoteur.
Statistiques : C’est ici que je veux pouvoir afficher des graphes et réaliser des enregistrements sur la consommation totale du PiRobot, les courbes de couples par moteurs, etc…
Options : Une page permettant de modifier certaines options de l’interface client ou du serveur sur le PiRobot, comme l’activation d’un mode Debug « pas à pas » pour analyser les trames dans la page de connexion.
Quitter : Ce bouton permettra de quitter l’application, car cette dernière est, bien entendue, lancée en plein écran sur ma tablette.

Maquette de la page de connexion (très sommaire ^-^)

Maquette de la page pilotage (très sommaire également)

Premiers essais graphiques sous Visual Studio 2017 RC (et Studio Blend)

Il reste également beaucoup à faire côté serveur :

[FAIT] Contrôle des moteurs
[FAIT] Serveur UDP
[FAIT] Interprétation des trames et multithreading
Retour d’information sur le couple moteur
Contrôle de la caméra
Retour caméra
Trame de retour UDP

3. Réflexions sur le pilotage du PiRobot

Comment sera piloté le PiRobot ? Ce dernier dispose de 4 moteurs :

Avant gauche
Arrière gauche
Avant droit
Arrière droit

Vu que les roues sont, de chaque côté, couplée par une chenille, on peut très bien, dans un premier temps piloter les moteurs droits et gauche ensemble :

Côté gauche (moteurs 2 et 4)
Côté droit (moteurs 1 et 3)

Je rappelle qu’un moteur est piloté comme suit :

Cmd_moteur(moteur, vitesse&sens) ;

Je peux sans problème demander aux moteurs d’avancer dans une direction, d’arrêter le PiRobot, d’établir un angle de 90° à droite et de reprendre tout droit. J’aurai effectué un virage à droite. Bien.

1er cas, très simple à gérer : aucun rayon de courbure, le robot effectue sa rotation sur place

Si maintenant je veux effectuer une courbe pour aller directement de l’étape 1 à l’étape 3 sans passer par l’étape 2, comment faire ? Comment réaliser une courbe ? Quel est le rayon de courbure que je peux réaliser ?

Dans la théorie, n’importe lequel.

Comportement plus naturel du PiRobot

Afin de réaliser ce comportement, il faudra que j’applique des coefficients de vitesse à chaque côté de moteurs. Et c’est ce coefficient qui va me déterminer ma courbe. Afin de définir la « force » du rayon de courbure, il faudra que je donne une valeur numérique à ma direction. C’est comme une voiture : plus vous tournez le volant, plus la voiture tourne. 3 choix s’offrent à moi :

Diminuer le coefficient de vitesse aux moteurs situés du côté intérieur au virage
Augmenter le coefficient de vitesse aux moteurs situés à l’extérieur du virage
Les deux point ci-dessus à la fois

La troisième possibilité permet de régler les soucis lorsque l’on arrive aux valeurs extrêmes de l’échelle PWM (0 et 4096). En effet, si on est à l’arrêt et que l’on veut effectuer un demi-tour sur soi-même la 3^ème option est préférable. Après pour les virages tout est une histoire de dosage. Une autre solution serait de contrôler, à la manière de certaines voitures téléguidées, la puissance moteur de chaque côté indépendamment et d’avoir 2 joysticks à un axe à contrôler, plutôt que deux boutons avancer/reculer et un « volant » gauche/droite, à la manière d’un jeu vidéo (qui était ma première approche).

Dans un jeu vidéo de voiture, le rayon de courbure d’une voiture qui tourne est calculée selon la vitesse du véhicule. Plus le véhicule sera rapide, plus il sera difficile de tourner. Ce comportement « arcade » tente de traduire le comportement réel de conduite. Or, avec le PiRobot, on ne dépassera pas les 15 km/h je pense, donc bon… Si je veux être précis, autant procéder de la sorte, et contrôler indépendamment chaque côté (avec un retour à 0 en cas de lâcher-prise, bien évidemment !).

Cet article m’a permis de poser la situation avant d’attaquer les prochaines grandes étapes de développement. De plus, vous êtes de plus en plus nombreux à me suivre et à me poser des questions sur ce projet ! Je vous invite à commenter ou à directement m’écrire à l’adresse suivante : contact_mail@bentek.fr

Bien cordialement, BenTeK.