---
title: "Asservissement"
date: "23-12-2020"
link: "https://hackmd.io/NIKLGipJSd61phS3N6HuWw"
tags: EVOLUTEK
---

# Asservissement

## Introduction

Ce document a pour but d'expliquer de quelle facon nous asservissons les robots Evolutek.

Un asservissement est un procédé pour corriger un système contrôlé en incluant l'erreur mesurée au cours du fonctionnement de ce système ([Wikipédia](https://fr.wikipedia.org/wiki/Asservissement_(automatique))).

(ex: Contrôle de température dans un grille-pain avec une mesure d'erreur avec un capteur de température)

Nous utilisons un asservissement pour contôller la position du robot lors des matchs. Cela nous permet de garantir la position du robot et de le faire se déplacer précisement.

## Carte moteur

La carte moteur est un contrôleur dual channel pour moteurs [DC](https://fr.wikipedia.org/wiki/Machine_%C3%A0_courant_continu) (Courant continu).

TODO : photo

Elle possède un [dsPIC33FJ128MC804](https://www.microchip.com/wwwproducts/en/dsPIC33FJ128MC804) qui s'occupe de faire tourner le système d'asservissement du robot.

La carte permet de communiquer en **CAN** ou en **RS232** mais nous n'utilisons que le **RS232** pour des raison de simplicité.

Elle possède aussi des interfaces **QEI** pour l'odométrie (voir ci-dessous).

Il y a aussi une protection en cas de sur-consommation des moteurs.

> A noter : La résistance R1 permet de faire en sorte de pouvoir allimenter la partie logique de la carte avec l'alimentation des moteurs, il faut l'enlever pour avoir une alimentation séparée.

## Odométrie

La position du robot est mesuré par des roues folles positionnées sur les côtés du robot (sur le même axe que les roues de déplacement) et montées sur des codeurs incrémentaux industriel ([Kubler 2400](https://www.kuebler.com/us/products/measurement/encoders/product-finder/product-details)). Ces encodeurs utilisent une interface **QEI** ([Quadrature Encoder Interface](http://ww1.microchip.com/downloads/en/DeviceDoc/70208A.pdf)) pour remonter le déplacement mesuré. Nous appelons cette mesure l'**Odométrie**.

Pour garantir une mesure continue est précise du déplacement du robot, les roues folles sont montées sur un système qui permet de les maintenir au contact du sol peut importe les conditions de la table et le déplacement du robot.

TODO : image des roues

## Localisation

Le robot est localisé sur la table avec trois coordonnées :
* $x$ en mm
* $y$ en mm
* $\theta$ en radian

La position du robot en $x$ et $y$ ne peut être négative.
L'angle du robot est toujours compris dans $[-\frac{\pi}{2};\frac{\pi}{2}]$.

L'origine de la table se place dans le coin en haut à gauche.


## Trajman

Trajman pour Trajectory Manager est notre système d'asservissement et de contrôle pour le déplacement de nos robots.

Le fonctionnement est plutôt simple :
Le système fait en sorte que le robot atteigne toujours une position ciblée.

Le robot ne s'arrêtent de bouger que lorsque :
* Il est à une position ciblée
* Il a été free
* Il a détecté une collision
* Il s'est recalé sur une bordure

### Architecture du code

L'architecture du code est plutôt simple :
* Un timer qui vient update toutes les 1ms :
* Les trapèzes des robots
* La position ciblée courante du robot
* Les QEI
* Une boucle principale qui :
* Lit les données recues sur le serial et applique les commandes
* Calcul la position courante du robot
* Update les commandes moteurs

La boucle principale est appelée le plus possible sans sleep.

### Commandes

A chaque itération de la boucle principale, le code vient lire les commandes recues sur le serial et les applique.

TODO : despcrition du protocôle
TODO : liste des commandes

### Boucle principale

TODO : schéma logique

### Calcul du de la nouvelle commande du robot

TODO

### Mise à jour de la position ciblée du robot

La position ciblée correspond à la position qu'est censé avoir atteint le robot après un laps de temps, pas la position finale.

Cette position est déterminée selon la vitesse à laquelle le robot est censé se déplacer à un instant $t$.

Nous avons donc :

$$
\theta Goal' = \theta Goal + \omega\\
xGoal' = xGoal + \cos(\theta Goal) * v\\
yGoal' = yGoal + \sin(\theta Goal) * v
$$

Avec :
* $\theta Goal'$ : nouvel angle à atteindre en radian
* $\theta Goal$ : ancien angle à atteindre en radian
* $\omega$ : vitesse angulaire actuelle du robot en radian/seconde
* $xGoal', yGoal'$ : nouvelle position cible en mm
* $xGoal, yGoal$ : ancienne position cible en mm
* $v$ : vitesse actuelle de translation en mm/seconde

### Rafraîchissement de l'odométrie

Un timer vient lire les valeur des QEI des encodeurs toutes les 1 ms.

Le calcul de la position du robot est fait à chaque tour de boucle.

Pour calculer la distance parcouru par une roue folle, il faut appliquer :

$$
dW = \pm \frac{nbTicks \times wDiam \times \pi}{4 \times resEncodeur}
$$

Avec :
* $dW$ : le déplacement de la roue en mm
* $nbTicks$ : le nombre de ticks de l'encodeur recu depuis le dernier calcul
* $wDiam$ : le diamètre de la roue en mm
* $resEncodeur$ : la résolution de l'encodeur (1024 pour notre modèle)

Grâce à cette formule, nous sommes capable de connaitre le déplacement de chaque roue folle du robot.

Nous pouvons ainsi obtenir le déplacement en translation avec cette formule :

$$
dTrsl = \frac{dL + dR}{2}
$$

Avec :
* $dTrsl$ : le déplacement du robot en translation en mm
* $dL$ : le déplacement de la roue gauche en mm
* $dR$ : le déplacement de la roue droite en mm

Pour obtenir le déplacement en rotation, nous utilisons la formule :

$$
dRot = \frac{dR - dL}{spacing}
$$

Avec :
* $dRot$ : le déplacement du robot en rotation en radian
* $dR$ : le déplacement de la roue droite en mm
* $dL$ : le déplacement de la roue gauche en mm
* $spacing$ : la distance entre les deux roues folles en mm

TODO : schèma

Pour obtenir la nouvelle position du robot, il suffit d'utiliser ces formules :

$$
\theta' = \theta + dRot\\
x = dTrsl \times \cos(\theta)\\
y = dTrsl \times \sin(\theta)
$$

Avec:
* $\theta'$ : le nouvel angle du robot en radian
* $\theta$ : l'ancien angle du robot en radian
* $dRot$ : le déplacement du robot en radian
* $x, y$ : la position du robot en mm
* $dTrsl$ : le déplacement en transaltion du robot en mm

### Asservissement

Nous asservissons le robot en position afin qu'il atteigne et se maintienne à une position définie (son objectif).

Nous utilisons une formules similaire à un PID classique, c'est une variation donnée par Mr. Benali, professeur de robotique à Epita.

Cette formule permet d'avoir un système plus stable puisqu'elle prend en compte la commande précédente et l'erreur précédente ainsi que l'erreur globale du système.

#### Calcul de l'erreur

L'erreur de translation est égale à :

$$
eTrsl = \sqrt{(xGoal - xPos)^2 + (yGoal - yGoal)^2}
$$

Avec :
* $eTrsl$ : l'erreur de positionent en mm
* $xGoal, yGoal$ : la position de l'objectif en mm
* $xPos, yPos$ : la position du robot en mm

L'erreur de rotation est égale à :

$$
eRot = \arctan(\frac{yGoal - yPos}{xGoal - xPos}) - \theta
$$

Avec :
* $eRot$ : l'erreur de rotation en radian
* $xGoal, yGoal$ : la position de l'objectif en mm
* $xPos, yPos$ : la position du robot en mm
* $\theta$ : l'angle du robot en radian

Il y a quelques cas particuliers :

##### Translation pure

TODO : application du trapèze

TODO : rajouter la condition où cette formule est appliquée

$$
eTrsl' = eTrsl * |{\cos(eRot)}|
$$

Avec :
* $eTrsl'$ : nouvelle erreur de translation en mm
* $eTrsl$ : l'erreur de translation calculée précédemment en mm
* $eRot$ : l'erreur de rotation calculée précédemment en radian

##### Robot arrêté ou en rotation pure

$$
eRot = \theta Goal - \theta Pos
$$

Avec :
* $eRot$ : l'erreur de rotation en radian
* $\theta Goal$ : l'angle de l'objectif en radian
* $\theta Pos$ : l'anle du robot en radian

Une fois ces erreurs calculé, nous changeons le sens du déplacement (translation ou rotation) si le robot doit reculler.

Nous applicons ensuite une projection orthonormale sur l'erreur de translation sur l'axe du robot afin d'annuler les effets de l'oscillation :

TODO : à préciser

$$
TODO
$$

#### Calcul et application de la commande

A la place d'un PID classique, nous utilisons comme formule :

$$
command(T) = command(T-1) + Kp \times (e(T) - e(T-1)) + \frac{Kp}{Ki} \times e(T) + Kp \times Kd \times (e(T) - 2 \times e(T-1) + e(T-2))
$$

Avec :
* $command(t)$ : La commande a un instant t
* $e(t)$ : l'erreur à un instant t
* $Kp$ : le coefficient propotionel
* $Ki$ : le coefficient integral
* $Kd$ : le coefficient dérivé

Nous calculons donc à chaque tour de boucle la nouvelle commande de translation et de rotation à appliquer en utilisant l'erreur calculée précédemment que nous transformons pour avoir une commande par moteur :

$$
commandMoteur(t) = commandeTrsl(t) \pm commandeRot(t)
$$

Avec :
* $commandMoteur(t)$ : commande du moteur à l'instant t
* $commandeTrsl(t)$ : commande de translation à l'instant t
* $commandeRot(t)$ : commande de rotation à l'instant t

Avant d'appliquer les commandes aux moteurs, nous vérifions qu'elles ne dépassent la commande maximum sinon nous réduisons propotionnellement les commandes des deux moteurs.

### Trapèzes

TODO

### Free/Unfree

Le free permet de désactiver l'asservissement selon nos besoins.

Il est utilisé notamment pour :
* Manipuler le robot
* L'empêcher de bouger
* Changer sa position

Si le robot est free, à chaque tour de boucle, on :
* Mets la commande des moteurs à 0
* Mets la position courante en position cible
* Reset les variables du PID

Pour ré-asservir le robot, il s'uffit de lui envoyer une commande de déplacement.

### Détection de collision

Nous sommes capables de détecter si le robot est entrer en collision en regardant si les roues codeuses n'ont pas bougés pendant un certain laps de temps.

Pour se faire, nous calculons l'erreur en translation et en rotation.

L'erreur de translation :

$$
eTrsl = \sqrt{(xGoal - xPos)^2 + (yGoal - yGoal)^2}
$$

Avec :
* $eTrsl$ : l'erreur de positionent en mm
* $xGoal, yGoal$ : la position de l'objectif en mm
* $xPos, yPos$ : la position du robot en mm

L'erreur de rotation :

$$
eRot = \theta Goal - \theta Pos
$$

Avec :
* $eRot$ : l'erreur de rotation en radian
* $\theta Goal$ : l'angle de l'objectif en radian
* $\theta Pos$ : l'anle du robot en radian

Pour savoir si une collision a eu lieu, nous comparons les erreurs avec des paramètres configurés (delta_trsl_max et delta_rot_max).

En cas de dépassement de ces paramètres, on determine qu'il y a une rotation et on de-asservit le robot.

### Recalage sur une bordure

Pour que le robot determine sa position automatiquement, nous avons implémenté une proccédure pour qu'il vienne se coller tout seul à une bordure.

En effet, le robot connaît sa taille (configurée dans les paramètres), donc lorsqu'il est collé à une bordure, il est positionné à :

$$
pos = bPos \pm robotSize
$$

Avec :
* $pos$ : la position du robot sur un axe (x ou y) en mm
* $bPos$ : la position de la bordure sur le même axe en mm
* $robotSize$ : la taille du robot en mm

Puisque le robot est aligné avec la bordure, nous en profitons pour configurer son angle.

TODO : schéma

D'un point de vue asservissement, nous ne faisons que :
* Désactiver l'asservissement de rotation pour qu'il puisse se coller au mur.
* Reculer vers la bordure
* Attendre d'être collé à la bordure
* Configurer la position du robot

Pour détecter que le robot est collé à la bordure, nous regardons si les codeuses n'ont pas été mise à jour dix fois d'affilée.

### Améliorations à apporter

#### Proccédure de recalage

Comme nous désactivons l'asservissement de rotation lorsqu'on recule sur la bordure, le robot à tendance à dériver et peut s'arrêter non collé au mur.

Il faudrait le désactiver une fois qu'une des codeuses à arrêter de s'update.

De plus, pour pouvoir calculer le gain des roues, nous avons besoin de désactiver la configuration de l'angle une fois le robot collé à la bordure.
Un premier essai à été fait mais ne fonctionne pas.

Il faudrait aussi essayer d'intégrer des capteurs pour améliorer la proccédure et permettre au robot de se recaller à distance des bordures.

Example :
TODO : schéma

#### Changement de vitesse pendant un déplacement

TODO

## Service Python

Pour communiquer avec la carte moteur, nous avons un service Python Trajman tournant sur le Linux du robot.

Ce service communique via un port serial avec l'asservissement pour lui envoyer des commandes et avoir des retours de la carte.

Grâce à ce service, nous pouvons envoyer toutes les commandes fournies par la carte moteur pour contrôller le robot.

Un thread s'occupe de lire les retours de la carte moteur et de soit :
* Les mettre dans une queue de réponse pour les actions appellées depuis Cellaserv.
* Les publier sur Cellaserv (ex : la télémétrie).

Le robot nous envoie en continue sa télémétrie afin de pouvoir le localiser sur la table. Elle contient :
* La position ($x, y$)
* L'angle ($\theta$)
* Le vitesse de translation

TODO : debug ?

Au démarrage du service, on vient configurer la carte moteur via les commandes set avec les paramètres stockés dans la configuration.

## Configuration du robot

Avant de pouvoir utiliser le robot à la coupe, nous avons besoin de comfigurer trois choses :

* Les PID du robots
* Le profil de vitesse du robot
* L'odométrie

### Odométrie

L'odométrie du robot se compose de trois paramètres :

* Le diamètre de la roue folle gauche en mm
* Le diamètre de la roue folle droite en mm
* L'espacement des roues folle en mm

Pour cela, il existe trois mèthodes pour obtenir :
* La différence de taille entre les deux roues folles
* Le diamètre moyen des deux roues folles
* L'espacement entre les deux roues folles

#### Calcul de la différence de taille entres les deux roues

Même si les deux roues folles sont fabriquées de la même facon, il y a toujours une différence de taille entre les deux.

Cette différence impacte la capacité du robot à suivre une trajectoire droite, ce qui est visible puisqu'il va dévier toujours dans la même direction.

Pour calculer cette différence, il faut mesurer l'angle obtenu après que le robot ait parcouru une grande distance dans un sens.

Voici la proccédure :
* Placer le robot contre un mur
* Recaler le robot sur le mur
* Le faire avancer sur une distance $d$
* Lui faire faire une rotation de 180 degrès
* Le faire revenir
* Lui faire faire une rotation de 180 degrès
* Recaler le robot contre le mur sans recaler son angle
* Mesurer la différence d'angle vu par le robot et celui de départ ($\Delta\theta$)

TODO schéma

Une fois cette proccédure faite, on peut calculer le facteur entre les deux roues :

$$
f = \frac{\Delta\theta}{d}
$$

Avec :
* $f$ : le facteur de différence de taille entre les deux roues
* $\Delta\theta$ : La différence d'angle entre l'angle de départ du robot et l'angle d'arrivé
* $d$ : La distance totale parcouru par le robot dans un sens

On peut ansi déterminer le nouveau gain d'une roue :

$$
gW'= (1 \pm f) \times gW
$$

Avec :
* $gW'$ : nouveau gain de la roue
* $f$ : le facteur de différence
* $gW$ : le gain précédent

#### Calcul du diamètre moyen des roues folles

Ce diamètre permet d'obtenir le diamètre de chacune des roues en utilisant les gains calculant précédement :

$$
dW = D \times gW
$$

Avec :
* $dW$ : le diamètre de la roue en mm
* $D$ : le diamètre moyen des roues en mm
* $gW$ : le gain de la roue

Pour obtenir le diamètre moyen, il suffit de pouvoir mesurer l'erreur du robot en translation. Le mieux étant de faire la plus grande translation possible.

Pour se faire :
* Placer le robot contre un mur
* Le recaler sur le mur
* Le faire avancer vers l'avant d'un certaine distance $d$
* Mesurer la distance parcourue par le robot ($d'$)

Le diamètre s'obtient par :
$$
D' = D \times \frac{d}{d'}
$$

Avec :
* $D'$ : le nouveau diamètre moyen en mm
* $D$ : l'ancien diamètre moyen en mm
* $d$ : la distance à parcourir en mm
* $d'$ : la distance parcourue en mm

#### Calcul de l'espacement des roues folles

Ce paramètre impacte la mesure de l'angle du robot et donc les rotations de celui-ci.

La proccédure pour le calculer est simple :
* Placer le robot sur un marquage pour soon angle
* Le faire tourner $n$ tours
* Mesurer la différence d'angle obtenue ($\Delta\theta$)

Pour calculer l'espacement du robot :

$$
s' = s \times (1 + \frac{\Delta\theta}{2 \times \pi \times n})
$$

Avec :
* $s'$ : le nouveau spacing en mm
* $s$ : l'ancien spacing en mm
* $\Delta\theta$ : la différence d'angle mesurée en radian
* $n$ : le nombre de tours

### Odom tools
TODO