View  Edit  Attributes  History  Attach  Print  Search

ACCUEIL | ARDUINO > S'INSTALLER > DEBUTER > APPROFONDIR | PROCESSING | MECATRONIQUE | MATERIEL | OUTILS | TESTS | Plus...|
Python > Shell > ATELIERS Python + Qt > PyQt apps > PyQt+Arduino | Mes Robots | RepRap | Mes lib'Arduino | Mes shields Arduino | Mes distros | Les Mini-PC |
ATELIERS ARDUINO| VIDEOS | COMPRENDRE | REFERENCES | CODER | TECHNIQUE | GNU/LINUX | LIENS | Rien à voir |

Bras robotisé 5 servomoteurs avec pince : Préhension de balle à à partir de la détection et du calcul de position d'une balle par webcam via une interface Processing de contrôle.

Par X. HINAULT - Page créée le : 08/08/2011.

1.  Présentation

Ce programme permet d'attraper une balle placée sur une zone cible à l'aide d'unbras articulé à 5 servomoteurs (1 servo base tournante, 3 servos de flexion et 1 servo de pince) par reconnaissance visuelle de la position de la balle. Le contrôle du bras est assuré par réception de chaines de caractères en provenance de l'interface Processing via le port série USB de la carte Arduino. Ainsi, les cinq servomoteurs connectés à la carte Arduino sont contrôlés à partir du PC.

1.1  Programme Arduino

Le contrôle du bras articulé à 5 servomoteurs se fait par chaine de la forme : servoto(i,xxx) avec xxx = angle en degrés.

Le code présenté ici utilise de nombreux tableaux avec une désignation des servomoteurs par indice : le code sera donc facilement transposable pour être utilisé avec 12, 20 servomoteurs ou plus !

Attention : trois chiffres obligatoires - pour 45 degrés on fera 045, pour -90, on fera -090

Les chaines reçues sur le port série décodées par le programme Arduino sont :

  • reset() : réinitialise position initiale du bras
  • vitesse(xxx) : paramétrage vitesse en ms - rapide = 5ms, lent 50ms, moyen 20ms
  • pas(xxx) : pas de psoitionnement en degrés. 1 degré par défaut
  • servo(i,xxx) : positionnement brut (et brutal...) d'un servomoteur à la valeur angle absolu
  • servoto(i,xxx) : positionnement progressif angle absolu
  • servotoR(i,xxx) : positionnement progressif angle relatif, valeur négative acceptée
  • servosBrasRSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle relatif - valeur négative acceptée
  • servosBrasSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle absolu

NB : Pendant l'exécution de ces 2 fonctions, tous les angles intermédiaires des servomoteurs sont affichés dans le Terminal !

  • Les valeurs négatives sont acceptées pour les angles relatifs

Ce programme utilise les fonctionnalités suivantes :

  • Utilise la connexion série vers le PC
  • Utilise 5 servomoteurs

Ressources utiles associées à ce programme :

  • La librairie Serial - pour les communications séries entre la carte Arduino et l'ordinateur ou d'autres composants
  • La librairie Servo - pour contrôler les servomoteurs.
  • La librairie Flash - hors référence - pour mettre simplement les chaînes de caractères et constantes de vos programmes dans la mémoire Flash programme de votre carte Arduino.

1.2  Le programme Processing

  • Ce programme Processing réalise d'une part la reconnaissance visuelle de la balle à saisir lorsqu'elle est placée sur la zone cible. Les coordonnées réelles en cm de la balle sont extraite de l'analyse de la position de la balle dans l'image vidéo issue de la webcam. On se base ici sur les programmes suivants :
  • Ce programme Processing réalise d'autre part le calcul automatisé des positions des 3 servomoteurs des segments du bras robotisé à partir de la simple connaissance de la distance à laquelle on veut que le bras de positionne (position de l'objet à prendre).
  • Les mathématiques sous-jacents à ce programme sont décrit ici : Math pour bras robotisé 3 segments
  • L'interface Processing va réaliser le tracé de la position du bras sous forme graphique
  • Le bras sera positionnable, pour un angle donné, par simple clic souris ou par saisie d'une valeur dans un champ texte.
  • Un bouton graphique permettra le positionnement souhaité et un autre permet de ramasser l'objet.

2.  Matériel Nécessaire

2.1  L'espace de développement Arduino

  • ... pour éditer, compiler le programme et programmer la carte Arduino.

2.2  Le matériel électronique suivant pour réaliser le montage associé

2.3  Les pièces mécaniques suivantes

  • Un kit bras robotisé 5 servomoteurs avec pince de chez http://www.easyrobotics.fr comportant :
    • 4 cages Easy standard + 1 grande cage Easy avec servomoteurs
    • 1 base pour bras avec plateau tournant
    • 1 rallonge bras
    • 1 pince avec engrenage et mords.

3.  Instructions de montage

  • La connexion série vers le PC utilise les broches 0 et 1 (via le câble USB)
  • Enficher la EasyCard broche à broche sur la carte Arduino
  • Connexion des servomoteurs :
    • connecter servo[0] = rotation base du bras sur la broche 6
    • connecter servo[1] = inclinaison base du bras sur la broche 7
    • connecter servo[2] = articulation médiane du bras sur la broche 8
    • connecter servo[3] = inclinaison pince du bras sur la broche 9
    • connecter servo[4] = ouverture pince du bras sur la broche 10

On utilise ici 5 servomoteurs dont un gros servomoteur pour l'axe de base du bras : IL EST IMPERATIF D'UTILISER UNE ALIMENTATION EXTERNE 5V pouvant supporter plusieurs ampères. Il faudra également toujours vérifier que cette alimentation est allumée avant de connecter le port USB de la carte Arduino, sinon c'est le port USB qui alimentera les servomoteurs... avec un risque de détérioration, bien que la carte Arduino dispose d'une protection par fusible intégré

  • Dans le cas d'une carte Arduino :
    • l'intensité maximale disponible sur une broche est de 40mA
    • l'intensité maximale cumulée pour l'ensemble des broches est 200mA
    • l'instensité maximale que peut fournir l'alimentation 5V de la carte est 500mA.
  • Par conséquent, avec une carte Arduino :
    • En ce qui concerne la ligne de commande du servomoteur par la carte Arduino :
      • on pourra commander directement autant de servomoteur que l'on veut (broche de commande) avec une carte Arduino, le nombre maximal étant 200mA / I commande, soit 100 environ dans le cas du Futaba S3003, ce qui est largement supérieur au nombre de broches de la carte.
    • Il n'y aura par ailleurs pas besoin d'amplification (type ULN 2803) sur la ligne de commande du servomoteur même si l'on utilise un grand nombre de servomoteurs.
    • En ce qui concerne l'alimentation principale des servomoteurs par une carte Arduino
      • on ne peut alimenter que 3 à 4 servomoteurs simultanément par l'alimentation 5V de la carte Arduino, le nombre maximal étant 500mA / I fonctionnement = 500 / 120 = 4 servomoteurs dans le cas du Futaba S3003.
      • Une alimentation externe sera indispensable dès que l'on dépassera ce nombre pour ne pas risquer de "griller" la carte Arduino.

Pour plus de détails, voir : Principes d'utilisation des alimentations avec une carte Arduino et des servomoteurs

4.  Le schéma théorique du montage

Le schéma théorique du montage (cliquer pour agrandir)

5.  Le circuit du montage

Le schéma du montage à réaliser (cliquer pour agrandir)

6.  Explications

6.1  Explications du programme Arduino

  • Le programme écoute le port série : lorsqu'une chaîne est reçue, elle est analysée et lorsqu'elle correspond à une chaine attendue, la fonction voulue de positionnement des servomoteurs est exécutée.
  • A noter que le programme est capable de recevoir une instruction à 5 paramètres sur le port série et d'en extraire correctement les 5 valeurs numériques voulues.
  • Le programme comprend plusieurs fonctions de positionnement des servomoteurs :
    • de façon progressive, en angle absolu ou relatif
    • de façon synchronisée pour plusieurs servomoteurs simultanément, là encore en angle absolu ou relatif.
  • Voir les commentaires du programme pour plus de détails.

6.2  Explications du programme Processing

  • Ce programme Processing réalise d'une part la reconnaissance visuelle de la balle à saisir lorsqu'elle est placée sur la zone cible. Les coordonnées réelles en cm de la balle sont extraite de l'analyse de la position de la balle dans l'image vidéo issue de la webcam. On se base ici sur les programmes suivants :
  • Ce programme Processing réalise d'autre part le calcul automatisé des positions des 3 servomoteurs des segments du bras robotisé à partir de la simple connaissance de la distance à laquelle on veut que le bras de positionne (position de l'objet à prendre).
  • L'interface Processing va réaliser le tracé de la position du bras sous forme graphique
  • Le bras sera positionnable par simple clic souris ou par saisie d'une valeur dans un champ texte.
  • Un bouton graphique permettra le positionnement souhaité et un autre permet de ramasser l'objet.
  • Le programme Processing envoie sur le port série les chaines voulues pour positionner le servomoteur
  • La valeur de l'ensemble des paramètres calculés par le programme sont affichés dans la console.
  • Tous les calculs des angles du bras se font automatique à partir de la position de l'objet calculée sur le flux vidéo webcam.

7.  Mise en oeuvre du programme

7.1  Préparation du montage et programmation de la carte Arduino :

  • Commencer par réaliser le montage indiqué sur plaque d'expérimentation
  • Ensuite, programmer la carte Arduino avec ce programme (en bas de page) selon la procédure habituelle

7.2  Fonctionnement

  • Programmer la carte Arduino puis lancer le programme Processing.
  • Etalonner la cible comme indiqué ici : Centrage de la webcam sur une cible carrée de taille connue
  • Positionner la balle sur la cible (balle de ping pong orangée) et clic sur Goto+ramasse : le bras se place au-dessus de la balle et ramasse.
  • L'interface Processing va réaliser le tracé de la position du bras sous forme graphique
  • Le bras sera positionnable par simple clic souris ou par saisie d'une valeur dans un champ texte.
  • Un bouton graphique permettra le positionnement souhaité et un autre permet de ramasser l'objet.
  • Le programme Processing envoie sur le port série les chaines voulues pour positionner le servomoteur
  • La valeur de l'ensemble des paramètres calculés par le programme sont affichés dans la console.

Se reporter au programme pour plus de détails.

8.  Le programme complet en langage Arduino

A copier/coller directement dans l'éditeur Arduino


// --- Programme Arduino ---
// Trame de code générée par le générateur de code Arduino
// du site www.mon-club-elec.fr

// Auteur du Programme : X. HINAULT - Tous droits réservés
// Programme écrit le : 25/06/2011.

// ------- Licence du code de ce programme -----
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License,
//  or any later version.
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.

// ////////////////////  PRESENTATION DU PROGRAMME ////////////////////

// -------- Que fait ce programme ? ---------
 /* Cinq servomoteurs connectés à la carte Arduino sont contrôlés
à partir du PC (interface Processing)

servo[0] = rotation base du bras
servo[1] = inclinaison base du bras
servo[2] = articulation médiane du bras
servo[3] = inclinaison pince du bras
servo[4] = ouverture pince du bras

Controle par chaine de la forme : servo(i,000) avec i = indice du servomoteur et 000 = angle en degrés

== Fonctions disponibles en saisie depuis le Terminal série pour le controle du bras :

reset() : réinitialise position initiale du bras

vitesse(xxx) : paramétrage vitesse en ms - rapide = 5ms, lent 50ms, moyen 20ms
pas(xxx) : pas de psoitionnement en degrés. 1 degré par défaut

servo(i,xxx) : positionnement brut (et brutal...) d'un servomoteur à la valeur angle absolu
servoto(i,xxx) : positionnement progressif angle absolu
servotoR(i,xxx) : positionnement progressif angle relatif, valeur négative acceptée

servosBrasRSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle relatif
servosBrasSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle absolu

// pendant l'exécution de ces 2 fonctions, tous les angles intermédiaires des servomoteurs sont affichés dans le Terminal !

NB : les fonctions saisies depuis la fenetre Terminal supportent les valeurs négatives
NB2 : les valeurs numériques doivent etre saisies avec 3 chiffres - mettre 0 devant au besoin



*/


// --- Fonctionnalités utilisées ---

// Utilise / fonctionne avec une interface Processing coté PC
// Utilise 5 servomoteurs
// Utilise le stockage des variables en mémoire Flash Programme


// -------- Circuit à réaliser ---------

// ******* ATTENTION : il est possible de connecter directement 2 ou 3 servomoteurs sur la carte Arduino (500mA maxi)
// au-delà utiliser une alimentation externe 5V

// connecter servo[0] = rotation base du bras sur la broche 6
// connecter servo[1] = inclinaison base du bras sur la broche 7
// connecter servo[2] = articulation médiane du bras sur la broche 8
// connecter servo[3] = inclinaison pince du bras sur la broche 9
// connecter servo[4] = ouverture pince du bras sur la broche 10

// /////////////////////////////// 1. Entête déclarative ///////////////////////
// A ce niveau sont déclarées les librairies incluses, les constantes, les variables, les objets utiles...

// --- Déclaration des constantes ---

// --- Inclusion des librairies ---

#include <Servo.h> // librairie pour servomoteur

#include <Flash.h> // Inclusion librairie pour stockage en mémoire Flash Programme
// Avant utilisation, il faut installer manuellement cette librairie
// dans le dossier <Libraries> du dossier Arduino
// infos : www.mon-club-elec.fr/pmwiki_reference_arduino/pmwiki.php?n=Main.LibrairieFlashProgramme

// --- Déclaration des constantes utiles ---
// const int APPUI=LOW; // constante pour tester état BP

//--- Constantes utilisées avec le servomoteur
const int ANGLE_MIN=0; // angle position MIN en degrés
const int POS_MIN=550; // largeur impulsion pour position ANGLE_MIN degrés du servomoteur
                  // POS_MIN=550 pour ANGLE_MIN=0 avec un futaba S3003

const int ANGLE_MAX=172; // angle position MAX en degrés
int POS_MAX=2400; // largeur impulsion pour position ANGLE_MAX degrés du servomoteur
                  // POS_MAX=2400 pour ANGLE_MAX=172 pour futaba S3003

// pour étalonner un servomoteur, voir la page :
//http://www.mon-club-elec.fr/pmwiki_mon_club_elec/pmwiki.php?n=MAIN.ArduinoExpertSerieDepuisPCPositionServomoteur

// --- Déclaration des constantes des broches E/S numériques ---

const int nbServos=5; // constante du nombre de servomoteurs

const int brocheServo[nbServos]={6,7,8,9,10}; // Tableau de constantes de broche

const float angleInitServo[nbServos]={90,120,10,10,45}; // tableau de constante de position initiale du servo  en degrés


// --- Déclaration des constantes des broches analogiques ---


// --- Déclaration des variables globales ---

 int octetReception=0; // variable de stockage des valeurs reçues sur le port Série
 long nombreReception=0; // variable de stockage du nombre  reçu sur le port Série
 long nombreReception0=0; // variable de stockage du dernier nombre  reçu sur le port Série
 String chaineReception=""; // déclare un objet String vide pour reception chaine

int valeur=0; // variable utile
long param=0; // paramètre transmis

long params[5]; // tableau de paramètres pour instructions à paramètres multiples

//---- initialisation valeur angles ---

float angleServo[nbServos]={90,90,90,90,90}; // tableau de variables de position du servo  en degrés

float angle0Servo[nbServos]={90,90,90,90,90}; // tableau de variable de la dernière position du servo  en degrés

int vitesse=10; // variable utilisée pour délai entre 2 positionnement servomoteur
int pas=20; // variable globale fixant nombre de pas pour positionnement progressif "to"

int compt=0; // variable util e
int pause=250; // variable utile

// --- Déclaration des objets utiles pour les fonctionnalités utilisées ---

//--- Création objet servomoteur

Servo servo[nbServos]; // crée un tableau d'objet servomoteur



// ////////////////////////// 2. FONCTION SETUP = Code d'initialisation //////////////////////////
// La fonction setup() est exécutée en premier et 1 seule fois, au démarrage du programme

void setup()   { // debut de la fonction setup()

// --- ici instructions à exécuter 1 seule fois au démarrage du programme ---

// ------- Initialisation fonctionnalités utilisées -------  

Serial.begin(115200); // initialise connexion série à 115200 bauds
// IMPORTANT : régler le terminal côté PC avec la même valeur de transmission

//--- Initialisation Servomoteur

// cf la dernière valeur utilisée avec write (ou writmicrosecond )est utilisée pour positionner les servomoteurs initialement
//--- position 90 par défaut à l'attache de la broche si rien de précisé ..

//---------- initialisation des servomoteurs ---------
for (int i=0; i<nbServos; i++) { // passe en revue les n servomoteurs

  //---- position initiale servo i ---
  servo[i].writeMicroseconds(angle(angleInitServo[i])); // crée impulsion à partir valeur angle - plus précis que write()
  //--- on positionne avant d'attacher le servomoteur à la broche sinon l'angle 90° est utilisé par défaut...

  servo[i].attach(brocheServo[i]);  // attache l'objet servo à la broche de commande du servomoteur

  //--- attention le positionnement initial est "brutal"

  //servoTo( servo1, angle0Servo1, angleInitServo1, vitesse, 1); //--- positionnement progressif par pas fixe de 1 degré ---
  angle0Servo[i]=angleInitServo[i];

} // fin for i initialisation



// ------- Broches en sorties numériques -------  

// ------- Broches en entrées numériques -------  

// ------- Activation si besoin du rappel au + (pullup) des broches en entrées numériques -------  


// ------- Initialisation des variables utilisées -------  


//-------- initialisation des broches ------

// ------- Codes d'initialisation utile -------  



//---------------- code exemple pour présension balle entre 2 positions - version lente----

/*
vitesse=15; // vitesse de positionnement initial rapide

delay(1000);
servosBrasSyncIndice(90,30,45,10,45); // baisse par dessus pince ouverte

delay(1000);
servosBrasSyncIndice(90,30,45,10,85); // idem ferme pince

delay(1000);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

delay(1000);
servosBrasSyncIndice(70,20,20,130,85); // pose pince fermée

delay(1000);
servosBrasSyncIndice(70,20,20,130,45); // pose pince ouverte

delay(1000);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée
*/


//---------------- code exemple pour présension balle entre 2 positions - version rapide----
/*
vitesse=2; // vitesse de positionnement initial rapide

delay(500);
servosBrasSyncIndice(90,30,45,10,45); // baisse par dessus pince ouverte

delay(500);
servosBrasSyncIndice(90,30,45,10,85); // idem ferme pince

delay(500);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

delay(500);
servosBrasSyncIndice(70,20,20,130,85); // pose pince fermée

delay(500);
servosBrasSyncIndice(70,20,20,130,45); // pose pince ouverte

delay(500);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

*/




} // fin de la fonction setup()
// ********************************************************************************

////////////////////////////////// 3. FONCTION LOOP = Boucle sans fin = coeur du programme //////////////////
// la fonction loop() s'exécute sans fin en boucle aussi longtemps que l'Arduino est sous tension

void loop(){ // debut de la fonction loop()


//---- code type réception chaine sur le port série ---
while (Serial.available()>0) { // tant qu'un octet en réception
        octetReception=Serial.read(); // Lit le 1er octet reçu et le met dans la variable

        if (octetReception==10) { // si Octet reçu est le saut de ligne
                Serial.print (F("Chaine recue=")),Serial.print(chaineReception); // affiche la chaine recue

                analyseChaine(chaineReception); // appelle la fonction d'analyse de la chaine en réception

                chaineReception=""; //RAZ le String de réception
                break; // sort de la boucle while
        }
        else { // si le caractère reçu n'est pas un saut de ligne
                chaineReception=chaineReception+char(octetReception); // ajoute le caratère au String
        }

} // fin tant que  octet réception

//----- une fois que le saut de ligne est reçu, on sort du While et on se positionne ici



//------------- fin analyse chaine ---------------




} // fin de la fonction loop() - le programme recommence au début de la fonction loop sans fin
// ********************************************************************************


// ////////////////////////// FONCTIONS DE GESTION DES INTERRUPTIONS ////////////////////


// ////////////////////////// AUTRES FONCTIONS DU PROGRAMME ////////////////////

//------------- fonction calibrage impulsion servomoteur à partir valeur angle en degrés

//------- mieux avec float -----
float angle(float valeur_angle) {

        float impuls=0;
        impuls=map(valeur_angle,ANGLE_MIN,ANGLE_MAX,POS_MIN, POS_MAX);
        return impuls;  

} // fin fonction impulsion servomoteur

//------------- fonction d'analyse de la chaine reçue sur le port série ----


//------------ analyseChaine ---------------
void analyseChaine(String chaineRecue) { // fonction d'analyse de la chaine recue

  // ---- analyse de la chaine recue sur le port Série ----
  chaineReception=chaineReception.trim(); // enlève les espaces

  //xxxxxxxxxxxxxxxxxxx instructions sans paramètres xxxxxxxxxxxx
 if (chaineReception=="reset()") { // si instruction reçue
    reset(); // exécute instruction si valide
  }


  //xxxxxxxxxxxxxxxxxxxx instructions avec paramètres xxxxxxxxxxxxxxx

     // info : la valeur numérique extraite par testInstruction() est stockée dans variable globale param

  //================= instructions paramètres généraux =============

  //-------------- test instruction pas(xxx) -----------
  if (testInstruction("pas(")==true) { // si instruction reçue valide

    pas=param; // change valeur pas

    Serial.print(F("pas = ")), Serial.println(pas);

  } // fin test pas(xxx)

  //-------------- test instruction vitesse(xxx) -----------
  if (testInstruction("vitesse(")==true) { // si instruction reçue valide


    vitesse=param; // change valeur vitesse (= durée delay en ms)
    Serial.print(F("vitesse = ")), Serial.println(vitesse);

  } // fin test vitesse(xxx)


    //--------------- test instruction servosBrasRSync(xxx,xxx,xxx,xxx,xxx) - position relative synchronisé

  if (testInstruction2("servosBrasRSync(",5)==true) { // si instruction avec 5 paramètres reçue valide


    //void servosRobotRSync( float S1, float S2, float S3, float S4, float S5) // reçoit les angles absolus des servomoteurs
    servosBrasRSyncIndice(float(params[0]),float(params[1]),float(params[2]),float(params[3]), float(params[4])); // positionnement synchronisé des servomoteurs
    // le nombre paramètres doit correspondre à la variable nbServos (nombre de servomoteurs)

    Serial.print(F("servosBrasRSync(")), Serial.print(params[0]),Serial.print(","), Serial.print(params[1]),Serial.print(",");
    Serial.print(params[2]),Serial.print(","), Serial.print(params[3]), Serial.print(","),Serial.print(params[4]), Serial.print(F(")"));

  } // fin test servoBrasRSync

    //--------------- test instruction servosBrasSync(xxx,xxx,xxx,xxx,xxx) - position absolue synchronisé

  if (testInstruction2("servosBrasSync(",5)==true) { // si instruction avec 5 paramètres reçue valide


    //void servosRobotRSync( float S1, float S2, float S3, float S4, float S5) // reçoit les angles absolus des servomoteurs
    servosBrasSyncIndice(float(params[0]),float(params[1]),float(params[2]),float(params[3]), float(params[4])); // positionnement synchronisé des servomoteurs
    // le nombre paramètres doit correspondre à la variable nbServos (nombre de servomoteurs)

    Serial.print(F("servosBrasSync(")), Serial.print(params[0]),Serial.print(","), Serial.print(params[1]),Serial.print(",");
    Serial.print(params[2]),Serial.print(","), Serial.print(params[3]), Serial.print(","),Serial.print(params[4]), Serial.print(F(")"));

  } // fin test servoBrasSync



  //================ instructions servo i =========

  //-------------- test instruction servo(i,xxx) ----------- // positionnement brut immédiat

  if (testInstruction2("servo(",2)==true) { // si instruction avec 2 paramètres reçue valide
  //format instruction (numéro servo, valeur angle absolu )
  // param[0] est le numéro du servomoteur, param[1] est la valeur de l'angle

    servo[params[0]].writeMicroseconds(angle(params[1])); // crée impulsion à partir valeur angle - plus précis que write()

    angleServo[params[0]]=params[1]; // mémorise angle actuel

  } // fin test servo1(xxx)


  //-------------- test instruction servoto(i,xxx) ----------- // positionnement progressif absolu
  if (testInstruction2("servoto(",2)==true) { // si instruction avec 2 paramètres reçue valide
  //format instruction (numéro servo, valeur angle absolu )
  // param[0] est le numéro du servomoteur, param[1] est la valeur de l'angle

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servo[params[0]], angle0Servo[params[0]], params[1], vitesse, 1); //--- positionnement progressif par pas fixe de 1 degré ---

    angle0Servo[params[0]]=params[1]; // met à jour l'angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F("angle0Servo")), Serial.print(params[0]),Serial.print(F(" = ")), Serial.println(angle0Servo[params[0]]);

  } // fin test servo1to(xxx)

  //-------------- test instruction servo1toR(i,xxx) ----------- // positionnement progressif relatif
  if (testInstruction2("servotoR(",2)==true) { // si instruction avec 2 paramètres reçue valide

//  if (testInstruction("servo1toR(")==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servo[params[0]], angle0Servo[params[0]], params[1], vitesse, 1); //--- positionnement progressif par pas fixe de 1 degré ---

    angle0Servo[params[0]]=angle0Servo[params[0]]+params[1]; // met à jour l'angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F("angle0Servo")), Serial.print(params[0]),Serial.print(F(" = ")), Serial.println(angle0Servo[params[0]]);

  } // fin test servo1toR(xxx)



} // ---------------- fin fonction analyse chaine ----------------------

//--------------- testInstruction : test si instruction de la forme instruction(xxx) ------------

boolean testInstruction(String chaineTest) { // reçoit chaine et renvoie true si instruction valide

  long posRef=chaineTest.length();// position de référence pour analyse (xxx)

  if (chaineReception.substring(0,posRef)==chaineTest) { // si reçoit l'instruction chaineTest(000)
  // nb substring : dernier caractere exclu

    Serial.print(F("Arduino va executer : "));

    Serial.print(chaineTest); // affiche

    if (chaineReception.substring(posRef,posRef+1)=="-") { // si signe - présent on décale de 1 position la prise en compte du nombre xxx

      param=(-1)*stringToLong(chaineReception.substring(posRef+1,posRef+4)); // extraction valeur 3 chiffres à partir caractères et * par -1
       posRef=posRef+1; // modif valeur posRef

    } // fin if

    else { // si pas de signe -

      param=stringToLong(chaineReception.substring(posRef,posRef+3)); // extraction valeur 3 chiffres à partir caractères

    } // fin else

    Serial.print(param); // affiche

    if (chaineReception.substring(posRef+3,posRef+4)==")") { // si fermeture parenthèse = instruction valide

      Serial.println(F(")")); // affiche
      Serial.println(F("Instruction valide !")); // affiche
      return(true); // renvoie true si instruction valide

    } // fin si fermeture parenthèse

    else {

      Serial.println(F("Instruction invalide !")); // affiche
      return(false); // renvoie false si instruction invalide

    } // fin else

  } // fin si reçoit l'instruction chaineTest(000)

} // fin fonction testInstruction  

//------------------- fin test instruction ------------


//--------------- testInstruction2 : test si instruction de la forme instruction(xxx, xxx, xxx, xxx, xxx) ou moins ------------

boolean testInstruction2(String chaineTest, int nbParam) { // reçoit chaine  et renvoie true si instruction valide

  long posRef=chaineTest.length();// position de référence pour analyse (xxx)

  if (chaineReception.substring(0,posRef)==chaineTest) { // si reçoit l'instruction chaineTest(000)
  // nb substring : dernier caractere exclu

    Serial.print(F("Arduino va executer : "));

    Serial.print(chaineTest); // affiche

    for (int i; i<nbParam; i++) { // défile les n paramètres

        if (chaineReception.substring(posRef,posRef+1)=="-") { // si signe - présent on décale de 1 position la prise en compte du nombre xxx

          posRef=posRef+1; // modif valeur posRef
          params[i]=(-1)*stringToLong(chaineReception.substring(posRef,posRef+3)); // extraction valeur 3 chiffres à partir caractères et * par -1

        } // fin if

        else { // si pas de signe -

          params[i]=stringToLong(chaineReception.substring(posRef,posRef+3)); // extraction valeur 3 chiffres à partir caractères

        } // fin else

        Serial.print(params[i]); // affiche

        if (chaineReception.substring(posRef+3,posRef+4)==",") { // si parenthèse attendue présente

          Serial.print(","); // affiche
          posRef=posRef+4; //décale position de référence de 4 caractères pour prise en compte nouvelle valeur

        } // fin if ","

    } // fin for nbParam

    if (chaineReception.substring(posRef+3,posRef+4)==")") { // si fermeture parenthèse = instruction valide

      Serial.println(F(")")); // affiche
      Serial.println(F("Instruction valide !")); // affiche
      return(true); // renvoie true si instruction valide

    } // fin si fermeture parenthèse

    else {

      Serial.println(F("Instruction invalide !")); // affiche
      return(false); // renvoie false si instruction invalide

    } // fin else

  } // fin si reçoit l'instruction chaineTest(000)

} // fin fonction testInstruction2  

//------------------- fin test instruction2 ------------



// ---------- fonction de conversion d'un String numérique en long

long stringToLong(String chaineLong) { // fonction conversion valeur numérique String en int

    long nombreLong=0; // variable locale
    int valeurInt=0; // variable locale

    for (int i=0; i<chaineLong.length(); i++) { // défile caractères de la chaine numérique

      valeurInt=chaineLong.charAt(i); // extrait le caractère ASCII à la position voulue - index 0 est le 1er caractère
      valeurInt=valeurInt-48; // obtient la valeur décimale à partir de la valeur ASCII  

     if (valeurInt>=0 && valeurInt<=9) { // si caractère est entre 0 et 9
       nombreLong=(nombreLong*10)+valeurInt;
     } // fin si caractère est entre 0 et 9


    } // fin for défile caractères

 return (nombreLong); // renvoie valeur numérique

} // ---------- fin stringToLong ------------


//--- fonction de positionnement progressif du servomoteur par pas fixe  -----

 void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas) {

       //--- positionnement progressif par pas fixe de 1 degré ---

    int delta=toAngle-fromAngle; // variation d'angle

    Serial.print(F("delta = ")), Serial.println(delta);

    if (delta>=0) { // si variation positive

      for (int i=0; i<delta; i++) { // defile n positions pour atteindre angle final dans sens positif

        fromAngle=fromAngle+1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle - plus précis que write()
        //Serial.print("angle courant servo = "), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin if

    else { // si variation négative

      for (int i=-delta; i>0; i--) { // defile n positions pour atteindre angle final dans sens négatif

        fromAngle=fromAngle-1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle - plus précis que write()
        //Serial.print("angle courant servo = "), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin else

 }

//--- fonction de positionnement progressif du servomoteur par pas fixe - angle relatif à la position courante  -----

 void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas) {

       //--- positionnement progressif par pas fixe de 1 degré ---

    int delta=toAngle; // variation d'angle correspond à l'angle transmis

    Serial.print(F("delta = ")), Serial.println(delta);

    if (delta>=0) { // si variation positive

      for (int i=0; i<delta; i++) { // defile n positions pour atteindre angle final dans sens positif

        fromAngle=fromAngle+1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle - plus précis que write()
        //Serial.print("angle courant servo = "), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin if

    else { // si variation négative

      for (int i=-delta; i>0; i--) { // defile n positions pour atteindre angle final dans sens négatif

        fromAngle=fromAngle-1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle - plus précis que write()
        //Serial.print("angle courant servo = "), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin else

 } // fin fonction servoToR


//------------- Fonctions positionnement synchronisé avec indices servomoteurs -------------------

//------------ fonction de positionnement synchronisé des servomoteurs du robot en poosition absolue ------
void servosBrasSyncIndice( float S1, float S2, float S3, float S4, float S5) { // reçoit les angles absolus des servos

  //------------- tableaux de valeurs utilisés par la fonction ---
  float S[nbServos]={S1,S2,S3,S4,S5}; // tableau de valeurs utilisant les paramètres reçus par la fonction

  float deltaS[nbServos]; // tableau de valeurs pour la différence entre l'angle courant et l'angle cible (deltas)
  float absDeltaS[nbServos]; // tableau de valeurs pour valeur absolues des deltas

  float cranS[nbServos];   // tableau de valeurs pour calcul des crans d'incrémentation pour chaque servomoteur

  //---- variables utilisées par la fonction
  float deltaMax=0;     //------ la plus grande valeur absolue des deltas

  //--------- calcul de la variation d'angle pour chaque servomoteur

  for (int i=0; i<nbServos; i++){

    deltaS[i]=S[i]-angle0Servo[i]; // -- calcule la différence entre l'angle courant et l'angle cible du servo i

    absDeltaS[i]=abs(deltaS[i]); // calcul de la valeur absolue du delta du servo i  
   //--- calculé ici pour éviter d'utiliser fonctions dans fonction max() - cf Référence

   //------ calcul du deltaMax = la plus grande valeur absolue des delta  
  deltaMax=max(deltaMax, absDeltaS[i]); // après tous les passages, la valeur la plus grande est conservée

  }

  Serial.print(F("deltaMax = ")), Serial.println(deltaMax); // affiche deltaMax

  // ---------- calcul des crans d'incrémentation pour chaque servomoteur ----
  //--- utilise delta avec signe +/- ---

    for (int i=0; i<nbServos; i++){

    cranS[i]=deltaS[i]/deltaMax; // divise delta / nombre de pas à effectuer par le servomoteur i

    Serial.print(F("cranS[")),  Serial.print(i), Serial.print(F("] = ")), Serial.println(cranS[i]);

    //-------- réinitialise variable angle courant des servomoteurs ----
    //- évite de modifier angle0Servo lors des calculs
    angleServo[i]=angle0Servo[i];

  } // fin for i nbServos



  //----------- défile les deltaMax positions et positionne les servomoteurs --------------
  for (int j=0; j<deltaMax; j++) { // parcourt les deltaMax crans

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

        //---------- servomoteur i
        angleServo[i]=angleServo[i]+cranS[i]; // ajoute cran
        servo[i].writeMicroseconds(angle(angleServo[i])); // crée impulsion à partir valeur angle - plus précis que write()
        Serial.print(F(" / S")),Serial.print(i), Serial.print(F(" = ")), Serial.print(angleServo[i]);


    } // fin for i nbServos

        //-------------- pause vitesse entre 2 positionnement des servomoteurs pour mouvement progressif
        Serial.println();
        delay(vitesse); // pause après positionnement de tous les servomoteurs

  } // fin for j deltaMax


    //------------- mise à jour variable des angles courants ------------------  
     //--- en se basant sur valeur angle de départ et delta ---
    //--- le résultat doit correspondre à celui obtenu par calculs précédents

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

         angle0Servo[i]=S[i]; // nouvel angle du servomoteur i
         Serial.print(F(" / S")),Serial.print(i), Serial.print(F("0 = ")), Serial.print(angle0Servo[i]);


    } // fin for i nbServos

    Serial.println();



} // fin servosRobotSyncIndice - fonction de positionnement synchronisé - angles en valeur absolue


//------------ fonction de positionnement synchronisé des servomoteurs du robot en position relative ------
void servosBrasRSyncIndice(float S1, float S2, float S3, float S4, float S5) { // reçoit les angles relatifs des servos
//------------ le nombre d'angle reçus par la fonction doit correspondre aux nombres de servomoteurs - constante nbServos

  //------------- tableaux de valeurs utilisés par la fonction ---
  float S[nbServos]={S1,S2,S3,S4,S5}; // tableau de valeurs utilisant les paramètres reçus par la fonction

  float deltaS[nbServos]; // tableau de valeurs pour la différence entre l'angle courant et l'angle cible (deltas)
  float absDeltaS[nbServos]; // tableau de valeurs pour valeur absolues des deltas

  float cranS[nbServos];   // tableau de valeurs pour calcul des crans d'incrémentation pour chaque servomoteur

  //---- variables utilisées par la fonction
  float deltaMax=0;     //------ la plus grande valeur absolue des deltas

  //--------- calcul de la variation d'angle pour chaque servomoteur

  for (int i=0; i<nbServos; i++){

    deltaS[i]=S[i]; // -- le delta est l'angle relatif envoyé à la fonction du servo i

    absDeltaS[i]=abs(deltaS[i]); // calcul de la valeur absolue du delta du servo i  
   //--- calculé ici pour éviter d'utiliser fonctions dans fonction max() - cf Référence

   //------ calcul du deltaMax = la plus grande valeur absolue des delta  
  deltaMax=max(deltaMax, absDeltaS[i]); // après tous les passages, la valeur la plus grande est conservée

  }


  Serial.print(F("deltaMax = ")), Serial.println(deltaMax); // affiche deltaMax

  // ---------- calcul des crans d'incrémentation pour chaque servomoteur ----
  //--- utilise delta avec signe +/- ---

    for (int i=0; i<nbServos; i++){

    cranS[i]=deltaS[i]/deltaMax; // divise delta / nombre de pas à effectuer par le servomoteur i

    Serial.print(F("cranS[")),  Serial.print(i), Serial.print(F("] = ")), Serial.println(cranS[i]);

    //-------- réinitialise variable angle courant des servomoteurs ----
    //- évite de modifier angle0Servo lors des calculs
    angleServo[i]=angle0Servo[i];

  } // fin for i nbServos


  //----------- défile les deltaMax positions et positionne les servomoteurs --------------
  for (int j=0; j<deltaMax; j++) { // parcourt les deltaMax crans

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

        //---------- servomoteur i
        angleServo[i]=angleServo[i]+cranS[i]; // ajoute cran
        servo[i].writeMicroseconds(angle(angleServo[i])); // crée impulsion à partir valeur angle - plus précis que write()
        Serial.print(F(" / S")),Serial.print(i), Serial.print(F(" = ")), Serial.print(angleServo[i]);



    } // fin for i nbServos

        //-------------- pause vitesse entre 2 positionnement des servomoteurs pour mouvement progressif
        Serial.println();
        delay(vitesse); // pause après positionnement de tous les servomoteurs

  } // fin for j deltaMax


    //------------- mise à jour variable des angles courants ------------------  
     //--- en se basant sur valeur angle de départ et delta ---
    //--- le résultat doit correspondre à celui obtenu par calculs précédents

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

         angle0Servo[i]=angle0Servo[i]+S[i]; // S[i]
         Serial.print(F(" / S")),Serial.print(i), Serial.print(F("0 = ")), Serial.print(angle0Servo[i]);


    } // fin for i nbServos

    Serial.println();


} // fin servosRobotRSyncIndice - fonction de positionnement synchronisé

//------------ fonction Reset ---------------

void reset(void) {

  servosBrasSyncIndice(angleInitServo[0],angleInitServo[1],angleInitServo[2],angleInitServo[3],angleInitServo[4]); // repositionne avec les angles initiaux


} // fin reset


// ////////////////////////// Fin du programme ////////////////////
 

9.  Le programme complet en langage Processing

9.1  Ressources utiles

  • Nécessite la librairie controlP5
  • Nécessite la librairie openCV

9.2  Le code

A copier/coller directement dans l'éditeur Processing


// Programme processing
// www.mon-club-elec.fr
// par X. HINAULT - Juillet -Aout 2011 - tous droits réservés

/////////////// Description du programme ////////////
// Utilise la souris
// Utilise le clavier
// Utilise la librairie GUI controlP5
// Utilise un/des bouton(s) simple(s) (Button)
// Utilise un/des champ(s) texte (Textfield)
// Utilise une/des fenetres supplémentaires

// ce programme réalise tout d'abord un traitement détecter l'objet de couleur voulu
// puis on utilise un calcul du ratio de l'aire de la forme / aire rectangle contenant
// pour améliorer la discrimination de l'objet
// le suivi d'objet final obtenu est très bon et les formes "aberrantes" sont ignorées
// les coordonnées de l'objet sont récupérées

// le bras robotisé utilise les coordonnées issues de l'analyse d'image pour prendre l'objet

/*

Calcul des angles d'un bras robotisé à 3 segments :
Positionnement du bras à la souris
Positionnement du bras par saisie position dans champ texte


Utilise calcul par théorème de Al-Kashi
Voir : http://fr.wikipedia.org/wiki/Th%C3%A9or%C3%A8me_d%27Al-Kashi
*/


// XXXXXXXXXXXXXXXXXXXXXX ENTETE DECLARATIVE XXXXXXXXXXXXXXXXXXXXXX

// inclusion des librairies utilisées

import processing.serial.*; // importe la librairie série processing

import controlP5.*; // importe la librairie GUI controlP5
// cette librairie doit être présente dans le répertoire /libraries du répertoire Processing
// voir ici : http://www.sojamo.de/libraries/controlP5/

import hypermedia.video.*; // importe la librairie vidéo et reconnaissance visuelle OpenCV
// cette librairie doit être présente dans le répertoire /libraries du répertoire Processing
// voir ici : http://ubaa.net/shared/processing/opencv/

import java.awt.Rectangle;
// impporte l'objet rectangle du langage Java qui est utilisé par certaines fonction de la librairie openCV
// l'objet rectangle fournit les champs x,y du centre et hauteur/largeur (height/width) du rectangle
// voir ici : http://download.oracle.com/javase/1.4.2/docs/api/java/awt/Rectangle.html


// déclaration objets

Serial myPort; // variable désignant le port série

//----- objets controlP5
ControlP5 controlP5; // déclare un objet principal de la librairie GUI controlP5

Button gotoButton; // déclare objet Button
Button ramasseButton; // déclare objet Button
Button gotoRamasseButton; // déclare objet Button

Toggle liveToggle; // Bouton ON/OFF pour activer le mode "live"

Textfield xpositionText; // déclare des objets Textfield
Textfield zpositionText; // déclare des objets Textfield

//ControlWindow cw1; // déclare un/des objet fenetre ControlWindow

PImage img1, img2; // déclare un/des objets PImage (conteneur d'image)

OpenCV opencv; // déclare un objet OpenCV principal


// déclaration variables globales
int xmin, xmax, ymin, ymax; // coordonnées de la zone à tester

float xCalc,yCalc, zCalc, angle0, angle; // variables utiles  

int[] xRef=new int[2]; // xRef pour la subdivision écran
int[] yRef=new int[2]; // yRef pour la subdivision écran

int[] subWidth=new int[2]; // width pour la subdivision écran
int[] subHeight=new int[2]; // height pour la subdivision écran

int video=0;
int graphe=1;

//--- correspondance fenêtre pixel / image cm
float Xmin=-2, Xmax=30.0; // abscisses limites du graphe
float Ymin=-2, Ymax=20.0; // ordonnées limites du graphe
// pour un tracé équilibré, il faut que Xmin-Xmax=Ymin-Ymax


//=============== variables globales de position utiles pour le bras robotisé ================

// le bras est constitué de 3 segments
// P1(x1,y1) - connu - est la base des 3 segments du bras robotisé
// P2(x2,y2) - à calculer - est le 2ème point du bras, séparé de P1 par la distance r1 (longueur du 1er segment)
// P3(x3,y3) - à calculer - est le 3ème point du bras, séparé de P2 par la distance r2 (longueur du 2ème segment)
// P(x,y) - connu - est le 4ème point du bras, séparé de P3 par la distance r3

// ==> P est le point cible correspondant à l'objet à saisir / centre de la pince

//--- paramètres mécaniques ---
float decalageXBras=3; // espace entre x1 et début zone graduée (bord base du bras)
float espaceMortX=5.5; // zone graduée inactive entre bras et zone graduée active
float zoneActiveX=18.0; // longueur graduée active au delà de l'espace mort

String angleBaseRamasse="170"; // angle pour lacher les balles
String angleBaseFace="083"; // angle axe bras en face

boolean positionInitiale=true; // drapeau de position initiale - vrai au démarrage
//passe à false après positionnement - repasse à true en position initiale

//--- les points du bras dans le plan (x,y) ---
float x1=0.0, y1=5.5; // coordonnées du point P1(x1,y1) - axe du servo 1 - connu
float x2, y2; // coordonnées du point P2(x2,y2) - axe du servo 2 - à calculer
float x3, y3; // coordonnées du point P3(x3,y3) - axe du servo 3 - à calculer

//---- le point cible P(x,y,z) - point central de la pince - connu = position de l'objet ---
float x=14.0; // coordonnées du point P sur l'axe Ox
float y=0.0; // coordonnées du point P sur l'axe Oy
float z=4.0; // coordonnée du point P(x,0,z) dans le plan (x,z) sur l'axe Oz.
// z est issu de l'analyse webcam de la position de l'objet

//--- les segments du bras ---
float r1=12.5;// longueur du 1er segment P1,P2 - connu
float r2=6.8; // longueur du 2ème segment P2,P3 - connu
float r3=8.8; // longueur du 3ème segment P3,P - connu
float r4; // longueur du segment P2,P - à calculer
float r; // longueur du segment P1,P - à calculer

float Rz; // longueur entre 0(0,0) et point P(x,0,z) dans le plan (x,z)
// Rz correspond à la valeur deltaX dans le plan "tournant" (x,y)

//--- les angles utiles du bras ---
// les angles sont exprimés en radians par défaut

//--- angles utiles pour le point P1(x1,y1)
float angleAlpha; // angle du servomoteur 1 - à calculer
float angleAlpha1; // angle pour calcul - à calculer
float angleAlpha2; // angle pour calcul - à calculer
float angleDeltaAlpha; // angle pour calcul - à calculer

//--- angles utiles pour le point P2(x2,y2)
float angleBeta; // angle du servomoteur 2- à calculer
float angleBeta1; // angle pour calcul - à calculer
float angleBeta2; // angle pour calcul - à calculer

//--- angles utiles pour le point P3(x3,y3)
float angleGamma; // angle du servomoteur 3 - à calculer
float angleGamma1; // angle pour calcul - à calculer

//---- angle utilisé dans le plan (x,z) pour position de la base rotative du bras
float angleTheta; // angle relatif du servomoteur 0 - base rotative du bras - à calculer
// l'angleTheta est relatif par rapport à l'angle int(angleBaseFace)
// qui est l'angle du servomoteur de la base tournant lorsque le bras est de face;

//--- autres variables utiles ---
float deltaX, deltaY; // variation X,Y des points P(x,y) et P1(x1,y1) dans le plan (x,y)- à calculer
float k=0.75; // coeff pour calcul aplha2 à partir deltaAlpha - connu

//============= fin des variables globales de position utiles pour le bras robotisé =============


//---- variable pour angle réel des servomoteurs ---
int[] angleServo=new int[5]; // tableau pour angles des servomoteurs

//------------------- variable pour la capture d'image ---
int comptImg=0; // variable de comptage des images pour moyenne centre
int nbImg=2; // nombre images à prendre en compte avant mouvement
long moyX=0; // pour calcul moyenne X
long moyY=0; // pour calcul moyenne Y


//------ déclaration des variables de couleur utiles ----
int jaune=color(255,255,0);
int vert=color(0,255,0);
int rouge=color(255,0,0);
int bleu=color(0,0,255);
int noir=color(0,0,0);
int blanc=color(255,255,255);
int bleuclair=color(0,255,255);
int violet=color(255,0,255);

//---------- variable pour la localisation dans l'image -----

//--- variables globales utiles
int tailleCroix=50; // 1/2 largeur bras croix

//--- pour étalonner ces valeurs, poser une règle devant la webcam
// et lire sur l'image vidéo la largeur en cm vue par la webcam

float largeurImageCm=30.5; // largeur en cm de l'image capturée par la webcam
float hauteurImageCm=23.0; // hauteur en cm de l'image capturée par la webcam


float tailleZoneUtileCm=18; // taille de la zone centrale carrée de travail utilisée
// idéalement cette zone ne doit pas dépasser 2/3 de la taille totale de l'image car les bords de l'image sont déformés par l'objectif

float xPixel, yPixel, xCm, yCm, xCmBras, yCmBras; // variables utiles

float[] xPixelCoin=new float[4]; // tableau pour les x des coins de la zone
float[] yPixelCoin=new float[4]; // tableau pour les y des coins de la zone


// XXXXXXXXXXXXXXXXXXXXXX  Fonction SETUP XXXXXXXXXXXXXXXXXXXXXX

void setup(){ // fonction d'initialisation exécutée 1 fois au démarrage


        // ---- initialisation paramètres graphiques utilisés
        colorMode(RGB, 255,255,255); // fixe format couleur R G B pour fill, stroke, etc...
        fill(0,0,255); // couleur remplissage RGB - noFill() si pas de remplissage
        stroke (0,0,0); // couleur pourtour RGB - noStroke() si pas de pourtour
        rectMode(CORNER); // origine rectangle : CORNER = coin sup gauche | CENTER : centre
        imageMode(CORNER); // origine image : CORNER = coin sup gauche | CENTER : centre
        ellipseMode(CENTER); // origine cercles / ellipses : CENTER : centre (autres : RADIUS, CORNERS, CORNER
        //strokeWeight(0); // largeur pourtour
        frameRate(5);// Images par seconde

        smooth();

        // --- initialisation fenêtre de base ---
        size(int(320*2.6),int(240*1.3)); // ouvre une fenêtre xpixels  x ypixels
//      size(int(320*4),int(240*2)); // ouvre une fenêtre xpixels  x ypixels
        background(blanc); // couleur fond fenetre

         //--- initialisation variables - à mettre après initialisaion fenêtre ++
        xRef[video]=0;
        println("xRef video ="+xRef[video]);

        xRef[graphe]=width/2;
        println("xRef graphe ="+xRef[graphe]);

        yRef[video]=0;
        yRef[graphe]=0;

        subWidth[graphe]=width/2;
        subWidth[video]=width/2;

        subHeight[graphe]=height;        
        subHeight[video]=height;        

// --- initialisation des objets et fonctionnalités utilisées ---

        //------------- initialisation port série ----
        println(Serial.list()); // affiche dans la console la liste des ports séries
        // Vérifier que le numéro du port série utilisé est le meme que celui utilisé avec  Serial.list()[index]
        myPort = new Serial(this, Serial.list()[0], 115200); // Initialise une nouvelle instance du port Série
        //myPort = new Serial(this, "/dev/ttyUSB0", 115200); // Initialise une nouvelle instance du port Série
        myPort.bufferUntil('\n'); // attendre arrivée d'un saut de ligne pour générer évènement série


        //======== Initialisation Objets GUI ControlP5 =========

        controlP5 = new ControlP5(this); // initialise l'objet principal de la librairie GUI controlP5

        // typeObjet nomObjet=controlP5.addObjet(paramètres); // pour info : déclaration / initialisation possible en 1 ligne
        // Textfield field = controlP5.addTextfield("myWindowTextfield",70,130,100,20); // exemple
        // addButton(String theName, float theValue, int theX, int theY, int theW, int theH)

        //======== Initialisation Objets Button =========


        // setImages(PImage theImageDefault,PImage theImageOver, PImage theImageActive,PImage theImageHighlight)
        // les images doivent etre de la meme taille que bouton, dans rép prog, type .jpg .png ..
        // un toggle n'utilise que image Default, Over et Active
        //b1.setImages(loadImage("imageDefault.png"),loadImage("imageOver.png"), loadImage("imageActive.png"),loadImage("imageDefault.png"));

        //---- le bouton goto
        gotoButton=controlP5.addButton("gotoButton",0,xRef[graphe]+150,yRef[graphe]+10,40,20); // initialise et ajoute un Button au ControlP5
        gotoButton.setLabelVisible(true); // affichage des labels
        gotoButton.setLabel("GOTO"); // fixe label du bouton
        gotoButton.setColorActive(color(255,0,0)); // fixe la couleur active
        gotoButton.setColorForeground(color(0,255,255)); // fixe couleur avant

        //---- le bouton ramasse
        ramasseButton=controlP5.addButton("ramasseButton",0,xRef[graphe]+200,yRef[graphe]+10,40,20); // initialise et ajoute un Button au ControlP5
        ramasseButton.setLabelVisible(true); // affichage des labels
        ramasseButton.setLabel("Ramasse !"); // fixe label du bouton
        ramasseButton.setColorActive(color(255,0,0)); // fixe la couleur active
        ramasseButton.setColorForeground(color(0,255,255)); // fixe couleur avant

        //---- le bouton goto + ramasse
        gotoRamasseButton=controlP5.addButton("gotoRamasseButton",0,xRef[graphe]+150,yRef[graphe]+50,80,20); // initialise et ajoute un Button au ControlP5
        gotoRamasseButton.setLabelVisible(true); // affichage des labels
        gotoRamasseButton.setLabel("Goto+ Ramasse !"); // fixe label du bouton
        gotoRamasseButton.setColorActive(color(255,0,0)); // fixe la couleur active
        gotoRamasseButton.setColorForeground(color(0,255,255)); // fixe couleur avant

        //======== Initialisation Objets Toggle =========

        // addToggle(String theName, boolean theDefaultValue, float theX, float theY, int theWidth, int theHeight)
        liveToggle=controlP5.addToggle("liveToggle",false,xRef[graphe]+250,yRef[graphe]+10,10,10); // initialise et ajoute un Button au ControlP5

        // méthodes propres à l'objet Toggle
        liveToggle.setMode(ControlP5.DEFAULT); // fixe le mode de fonctionnement du Toggle : ControlP5.DEFAULT ou ControlP5.SWITCH

        // méthodes communes à tous les controles (objet Controller)
        liveToggle.setLabelVisible(true); // affichage des labels
        liveToggle.setLabel("Live"); // fixe label objet
        liveToggle.setDecimalPrecision(2); // fixe la précision
        liveToggle.setColorActive(rouge); // fixe la couleur active
        liveToggle.setColorBackground(bleu); // fixe couleur fond  
        liveToggle.setColorForeground(color(0,0,255)); // fixe couleur avant
        liveToggle.setColorCaptionLabel(noir); // fixe couleur Label
        liveToggle.setColorValueLabel(color(0,0,255)); // fixe la couleur valeur

        // setImages(PImage theImageDefault,PImage theImageOver, PImage theImageActive,PImage theImageHighlight)
        // les images doivent etre de la meme taille que bouton, dans rép prog, type .jpg .png ..
        // un toggle n'utilise que image Default et Active
        //t1.setImages(loadImage("imageDefault.png"),loadImage("imageDefault.png"), loadImage("imageActive.png"),loadImage("imageDefault.png"));

        //======== Initialisation Objets Textfield =========

        //---- champ texte saisie valeur position
        xpositionText=controlP5.addTextfield("xpositionText",xRef[graphe]+10,yRef[graphe]+10,100,20); // initialise et ajoute un Textfield au ControlP5
        xpositionText.setAutoClear(false); // autoeffacement après return
        xpositionText.setValue("10"); // initialise Texte du champ
        xpositionText.setLabelVisible(true); // affichage des labels
        xpositionText.setLabel("Position x en cm (8.5-26.5cm)"); // fixe label
        xpositionText.setColorActive(color(255,0,0)); // fixe la couleur active
        xpositionText.setColorForeground(color(0,255,255)); // fixe couleur avant
        xpositionText.setColorCaptionLabel(bleu); // fixe couleur Label

        //---- champ texte angle ------
        zpositionText=controlP5.addTextfield("zpositionText",xRef[graphe]+10,yRef[graphe]+50,100,20); // initialise et ajoute un Textfield au ControlP5
        zpositionText.setAutoClear(false); // autoeffacement après return
        zpositionText.setValue("0"); // initialise Texte du champ
        zpositionText.setLabelVisible(true); // affichage des labels
        zpositionText.setLabel("Position z en cm (-9/+9cm)"); // fixe label
        zpositionText.setColorActive(color(255,0,0)); // fixe la couleur active
        zpositionText.setColorForeground(color(0,255,255)); // fixe couleur avant
        zpositionText.setColorCaptionLabel(bleu); // fixe couleur Label

        //======== Initialisation Objets ControlWindow =========

/*
        // addControlWindow(String theWindowName,int theX, int theY, int theWidth, int theHeight,int theFrameRate)
        cw1=controlP5.addControlWindow("fenetre",100,100,400,200);// ajoute une fenetre au ControlP5

        // méthodes propres à l'objet ControlWindow
        cw1.hideCoordinates(); //masque les coordonnées
        cw1.setBackground(noir);
        cw1.frameRate(10); // fixe le nombre de rafraichissement par seconde de la fenetre
        //cw1.setColorActive(color(255,0,0)); // fixe la couleur active
        cw1.setTitle("Ma fenetre"); // titre de la fenetre
        //cw1.setLocation(100, 100) ; // fixe la localisation dans la fenetre ?
        //cw1.setUndecorated(true); // fixe la bordure de la fenêtre

        // ajout de controles à la fenetre ControlWindow
        // nomObjet.setWindow(cw1); // met l'objet dans la fenêtre
        // b1.setWindow(cw1); // met l'objet dans la fenêtre
        gotoButton.setWindow(cw1); // met l'objet dans la fenêtre
        positionText.setWindow(cw1); // met l'objet dans la fenêtre
*/



        //======== Initialisation Objets OpenCV (vidéo et reconnaissance visuelle =========

        opencv = new OpenCV(this); // initialise objet OpenCV à partir du parent This
        opencv.capture(subWidth[video],subHeight[video],0); // initialise capture flux vidéo
        // width et height sont les valeurs de la taille de la fenêtre processing

//------------------ initialisation programme Arduino ----
        myPort.write("vitesse(015)\n");// fixe la vitesse de positionnement du servomoteur par l'Arduino


//------ calcul des coordonnées des coins de la zone utile centrée sur le centre de l'image ---

        //--- point angle sup gauche ---
        xCm=(largeurImageCm/2)-(tailleZoneUtileCm/2);
        println("xCm="+xCm);

        xPixelCoin[0]=map(xCm,0,largeurImageCm,0,subWidth[video]);
        println("xPixel 0="+xPixelCoin[0]);

        yCm=(hauteurImageCm/2)-(tailleZoneUtileCm/2);
        println("yCm="+yCm);

        yPixelCoin[0]=map(yCm,0,hauteurImageCm,0,subHeight[video]);
        println("yPixel 0="+yPixelCoin[0]);

        //--- point angle sup droit ---
        xCm=(largeurImageCm/2)+(tailleZoneUtileCm/2);
        println("xCm="+xCm);

        xPixelCoin[1]=map(xCm,0,largeurImageCm,0,subWidth[video]);
        println("xPixel 1="+xPixelCoin[1]);

        yCm=(hauteurImageCm/2)-(tailleZoneUtileCm/2);
        println("yCm="+yCm);

        yPixelCoin[1]=map(yCm,0,hauteurImageCm,0,subHeight[video]);
        println("yPixel 1 ="+yPixelCoin[1]);

        //--- point angle inf gauche ---
        xCm=(largeurImageCm/2)-(tailleZoneUtileCm/2);
        println("xCm="+xCm);

        xPixelCoin[2]=map(xCm,0,largeurImageCm,0,subWidth[video]);
        println("xPixel 2="+xPixelCoin[2]);

        yCm=(hauteurImageCm/2)+(tailleZoneUtileCm/2);
        println("yCm="+yCm);

        yPixelCoin[2]=map(yCm,0,hauteurImageCm,0,subHeight[video]);
        println("yPixel 2="+yPixelCoin[2]);

        //--- point angle inf droit ---
        xCm=(largeurImageCm/2)+(tailleZoneUtileCm/2);
        println("xCm="+xCm);

        xPixelCoin[3]=map(xCm,0,largeurImageCm,0,subWidth[video]);
        println("xPixel 3="+xPixelCoin[3]);

        yCm=(hauteurImageCm/2)+(tailleZoneUtileCm/2);
        println("yCm="+yCm);

        yPixelCoin[3]=map(yCm,0,hauteurImageCm,0,subHeight[video]);
        println("yPixel 3="+yPixelCoin[3]);

//------ code initialisation utile ----

//---- calcul initial des variables globales de position du bras robotisé ---

//calculAngles(x, y); // calcul des variables globales de position du bras robotisé à partir coordonnées du point cible dans le plan x,y

calculAngles(x,y,z); // calcul des variables globales de position du bras robotisé à partir coordonnées du point cible dans le plan x,y,z

//------------ dessin initial -------------

traceBras(); // tracé initial du Bras robotisé 3 segments
//traceBrasZ(); // tracé initial du Bras robotisé 3 segments

//---- test de calcul ----

//println("xLine="+xLinePolaireRad(2,3,5.83,radians(56.31),radians(25.35)));//xLinePolaireRad(xo,yo,r,angle0,angle)

//println("yLine="+yLinePolaireRad(2,3,5.83,radians(56.31),radians(25.35)));//yLinePolaireRad(xo,yo,r,angle0,angle)



} // fin fonction Setup

// XXXXXXXXXXXXXXXXXXXXXX Fonction Draw XXXXXXXXXXXXXXXXXXXX

void  draw() { // fonction exécutée en boucle



analyseWebcam();


} // fin de la fonction draw()

// XXXXXXXXXXXXXXXXXXXXXX Autres Fonctions XXXXXXXXXXXXXXXXXXXXXX

//pour les fonctions de conversion, on considère que le point 0,0 est le coin inf gauche de la fenêtre
// important dans le cas de y

//----- Conversion xCmtoPixel() -----

float xCmtoPixel(float xIn) {

  float xOut;
  xOut=map(xIn,Xmin,Xmax,0,subWidth[graphe]);
  return (xOut);

}


//----- Conversion yCmtoPixel() -----

float yCmtoPixel(float yIn) {

  float yOut;
  yOut=map(yIn,Ymin,Ymax,0,subHeight[graphe]);
  yOut=subHeight[graphe]-yOut;
  return (yOut);

}


//----- Conversion xPixeltoCm() -----

float xPixeltoCm(float xIn) {

  float xOut;
  xOut=map(xIn,0,subWidth[graphe],Xmin,Xmax);
  return (xOut);

}

//----- Conversion yPixeltoCm() -----

float yPixeltoCm(float yIn) {

  float yOut;
  yIn=subHeight[graphe]+yIn;
  yOut=map(yIn,0,subHeight[graphe],Ymin,Ymax);
  return (yOut);

}

        //------------- Fonction de gestion des évènements série ----
         void serialEvent (Serial myPort) { // fonction appelée lors de la survenue d'un évènement série

        // ******** Gestion de la valeur reçue sur le port série : **********

        String inString = myPort.readStringUntil('\n'); // chaine stockant la chaîne reçue sur le port Série
        // saut de ligne en marque de fin

        if (inString != null) { // si la chaine recue n'est pas vide


                print (inString); // affichage brut de la chaine recue


        } // fin condition chaine recue pas vide


} // fin de la fonction de gestion des évènements Série



//===================== Fonction de calcul des angles : calculAngles(float xIn, float yIn, float zIn) ================

// Cette fonction utilise les variables globales de position du bras robotisé suivantes :

/*

//=============== variables globales de position utiles pour le bras robotisé ================

// le bras est constitué de 3 segments
// P1(x1,y1) - connu - est la base des 3 segments du bras robotisé
// P2(x2,y2) - à calculer - est le 2ème point du bras, séparé de P1 par la distance r1 (longueur du 1er segment)
// P3(x3,y3) - à calculer - est le 3ème point du bras, séparé de P2 par la distance r2 (longueur du 2ème segment)
// P(x,y,z) - connu - est le 4ème point du bras, séparé de P3 par la distance r3

// ==> P est le point cible correspondant à l'objet à saisir / centre de la pince

//--- les points du bras ---
float x1=0.0, y1=5.5; // coordonnées du point P1(x1,y1) - axe du servo 1 - connu
float x2, y2; // coordonnées du point P2(x2,y2) - axe du servo 2 - à calculer
float x3, y3; // coordonnées du point P3(x3,y3) - axe du servo 3 - à calculer

//---- le point cible P(x,y,z) - point central de la pince - connu = position de l'objet ---
float x=15.0; // coordonnées du point P sur l'axe Ox
float y=0.0; // coordonnées du point P sur l'axe Oy
float z; // coordonnée du point P(x,0,z) dans le plan (x,z) sur l'axe Oz.
// z est issu de l'analyse webcam de la position de l'objet

//--- les segments du bras ---
float r1=12.5;// longueur du 1er segment P1,P2 - connu
float r2=6.8; // longueur du 2ème segment P2,P3 - connu
float r3=8.8; // longueur du 3ème segment P3,P - connu
float r4; // longueur du segment P2,P - à calculer
float r; // longueur du segment P1,P - à calculer

float Rz; // longueur entre 0(0,0) et point P(x,0,z) dans le plan (x,z)
// Rz correspond à la valeur deltaX dans le plan "tournant" (x,y)

//--- les angles utiles du bras ---
// les angles sont exprimés en radians par défaut

//--- angles utiles pour le point P1(x1,y1)
float angleAlpha; // angle du servomoteur 1 - à calculer
float angleAlpha1; // angle pour calcul - à calculer
float angleAlpha2; // angle pour calcul - à calculer
float angleDeltaAlpha; // angle pour calcul - à calculer

//--- angles utiles pour le point P2(x2,y2)
float angleBeta; // angle du servomoteur 2- à calculer
float angleBeta1; // angle pour calcul - à calculer
float angleBeta2; // angle pour calcul - à calculer

//--- angles utiles pour le point P3(x3,y3)
float angleGamma; // angle du servomoteur 3 - à calculer
float angleGamma1; // angle pour calcul - à calculer

//---- angle utilisé dans le plan (x,z) pour position de la base rotative du bras
float angleTheta; // angle relatif du servomoteur 0 - base rotative du bras - à calculer
// l'angleTheta est relatif par rapport à l'angle int(angleBaseFace)
// qui est l'angle du servomoteur de la base tournant lorsque le bras est de face;


//--- autres variables utiles ---
float deltaX, deltaY; // variation X,Y des points P(x,y) et P1(x1,y1) - à calculer
float k=0.75; // coeff pour calcul aplha2 à partir deltaAlpha - connu

//============= fin des variables globales de position utiles pour le bras robotisé =============

*/



void calculAngles(float xIn, float yIn, float zIn) { // la fonction reçoit les coordonnées à utiliser pour le point P

//------------ coordonnées du point P(x,y) de référence
  x=xIn; // utilise xIn comme abscisse du point P ( x est une variable globale )
  y=yIn; // utilise yIn comme ordonnée du point P (y est un variable globale)
  z=zIn; // utilise zIn comme ordonnée du point P (z est un variable globale)

  println("x:y:z="+x+":"+y+":"+z);

//-------------- calcul de Rz ------
Rz=sqrt(pow(x,2)+pow(z,2)); // calcul de la distance O - P dans le plan x,z
println("Rz="+Rz);

//---- calcul de l'angle theta qui correspond à l'angle de rotation du servomoteur de la base rotative du bras ---
angleTheta=asin(z/Rz);
println("angle Theta="+degrees(angleTheta));

//------------- séquence de calcul des angles à partir de Rz,y ----

//----- calcul de deltaX et deltaY ----

//deltaX=x-x1;
deltaX=Rz; // on suppose que x1=0
println ("deltaX="+deltaX);

deltaY=y-y1;
println ("deltaY="+deltaY);

//---- calcul angle alpha1

angleAlpha1=abs(atan(deltaX/deltaY));
println ("alpha1="+degrees(angleAlpha1));

//---- calcul de r ----
r=sqrt(pow(deltaX,2)+pow(deltaY,2));
println ("r="+r);

//--- calcul de delta Alpha ---

angleDeltaAlpha=calculAngleRadAlKashi(r1,r,r2+r3); // float calculAngleRadAlKashi( float adj1, float adj2, float opp)
println ("delta alpha= "+degrees(angleDeltaAlpha));

//--- calcul de alpha2 ---
angleAlpha2=angleDeltaAlpha*k;
println ("alpha 2 = "+degrees(angleAlpha2));

//--- calcul de alpha ---
angleAlpha=angleAlpha1+angleAlpha2;
println ("alpha = "+degrees(angleAlpha));

//--- calcul de r4 ----
r4=calculCoteRadAlKashi(r1,r,angleAlpha2); // float calculCoteRadAlKashi( float adj1, float adj2, float angleOpp)
println ("r4 ="+r4);

//--- calcul beta1 ---
angleBeta1=calculAngleRadAlKashi(r1,r4,r); // float calculAngleRadAlKashi( float adj1, float adj2, float opp)
println ("angle beta 1 = "+degrees(angleBeta1));

//--- calcul beta2 ---
angleBeta2=calculAngleRadAlKashi(r2,r4,r3); // float calculAngleRadAlKashi( float adj1, float adj2, float opp)
println ("angle beta 2 = "+degrees(angleBeta2));

//--- calcul beta ---
angleBeta=PI-angleBeta1-angleBeta2;
println ("angle beta  = "+degrees(angleBeta));

//--- calcul Gamma 1 ---
angleGamma1=calculAngleRadAlKashi(r2,r3,r4); // float calculAngleRadAlKashi( float adj1, float adj2, float opp)
println ("angle Gamma 1 = "+degrees(angleGamma1));

//--- calcul Gamma ---
angleGamma=PI-angleGamma1;
println ("angle Gamma  = "+degrees(angleGamma));


} //====================fin de la fonction calculAngles(xIn, yIn) ============


//===================== fonction de tracé des lignes du bras robotisé ===============

/*
Cette fonction trace les points et lignes du bras robotisé dans la fenêtre graphique
à partir de la valeur courante des variables globales de position du bras robotisé
*/


void traceBras() { // la fonction de reçoit aucun paramètre

    fill(blanc);
    rect(xRef[graphe],yRef[graphe],subWidth[graphe],subHeight[graphe]); // trace rectangle pour effacer sous fenêtre

    noFill();
    stroke (bleu); // couleur pourtour RGB - noStroke() si pas de pourtour

    // --- tracé des éléments fixes connus ---

    // point origine O(0,0)    
    //point(map(0,Xmin,Xmax,0,width),height-map(0,Ymin,Ymax,0,height)); // O(0,0)
    //ellipse(map(0,Xmin,Xmax,0,width),height-map(0,Ymin,Ymax,0,height),5,5); // O(0,0)
    ellipse(xRef[graphe]+xCmtoPixel(0),yRef[graphe]+yCmtoPixel(0),5,5); // O(0,0)    

    // point P1
    //point(map(x1,Xmin,Xmax,0,width),height-map(y1,Ymin,Ymax,0,height)); // P1
    //ellipse(map(x1,Xmin,Xmax,0,width),height-map(y1,Ymin,Ymax,0,height),5,5); // P1
    ellipse(xRef[graphe]+xCmtoPixel(x1),yRef[graphe]+yCmtoPixel(y1),5,5); // P1


    // ellipse(x, y, width, height)
    // map(value, low1, high1, low2, high2)

    // cercle C1
    ellipse(xRef[graphe]+xCmtoPixel(x1),yRef[graphe]+yCmtoPixel(y1),2*r1/(Xmax-Xmin)*subWidth[graphe],2*r1/(Ymax-Ymin)*subHeight[graphe]); // cercle C1

    // point P(x,y)
    ellipse(xRef[graphe]+xCmtoPixel(Rz),yRef[graphe]+yCmtoPixel(y),5,5); // P
    //---- utilise Rz et pas x

    // --- tracé des distances de la zone utile ---

    line(xRef[graphe]+xCmtoPixel(0+decalageXBras+espaceMortX),yRef[graphe]+yCmtoPixel(0),xRef[graphe]+xCmtoPixel(0+decalageXBras+espaceMortX+zoneActiveX),yRef[graphe]+yCmtoPixel(0)); // Zone active

    //---- affichage des positions tous les cm
    for (int i=0; i<=zoneActiveX; i++) {
      ellipse(xRef[graphe]+xCmtoPixel(i+decalageXBras+espaceMortX),yRef[graphe]+yCmtoPixel(0),5,5); // Positions tous les cm zone active
    }

    // --- tracé des éléments obtenu par calcul ---

    // tracé de P1,P2
    angle0=0; // premier point
    angle=HALF_PI-angleAlpha; // utilise AngleAlpha par rapport à l'horizontale

    x2=xLinePolaireRad(x1,y1,r1,angle0,angle);//xLinePolaireRad(xo,yo,r,angle0,angle)
    y2=yLinePolaireRad(x1,y1,r1,angle0,angle);//xLinePolaireRad(xo,yo,r,angle0,angle)

    //println ("x2 ="+x2);
    //println ("y2 ="+y2);

    stroke(violet);
    ellipse(xRef[graphe]+xCmtoPixel(x2),yRef[graphe]+yCmtoPixel(y2),5,5); // P2

    stroke(bleu);
    strokeWeight(10); // largeur pourtour
    line(xRef[graphe]+xCmtoPixel(x1),yRef[graphe]+yCmtoPixel(y1),xRef[graphe]+xCmtoPixel(x2),yRef[graphe]+yCmtoPixel(y2));
    strokeWeight(1); // largeur pourtour

    stroke(violet);
    ellipse(xRef[graphe]+xCmtoPixel(x2),yRef[graphe]+yCmtoPixel(y2),2*r2/(Xmax-Xmin)*subWidth[graphe],2*r2/(Ymax-Ymin)*subHeight[graphe]); // cercle C2


    // tracé de P2,P3
    angle0=angle0-angle; // point précédent
    angle=angleBeta; // utilise AngleBeta

    x3=xLinePolaireRad(x2,y2,r2,angle0,angle);//xLinePolaireRad(xo,yo,r,angle0,angle)
    y3=yLinePolaireRad(x2,y2,r2,angle0,angle);//xLinePolaireRad(xo,yo,r,angle0,angle)

    //println ("x3 ="+x3);
    //println ("y3 ="+y3);

    stroke(rouge);
    ellipse(xRef[graphe]+xCmtoPixel(x3),yRef[graphe]+yCmtoPixel(y3),5,5); // P3

    stroke(violet);
    strokeWeight(10); // largeur pourtour
    line(xRef[graphe]+xCmtoPixel(x2),yRef[graphe]+yCmtoPixel(y2),xRef[graphe]+xCmtoPixel(x3),yRef[graphe]+yCmtoPixel(y3)); // r2
    strokeWeight(1); // largeur pourtour

    stroke(rouge);
    ellipse(xRef[graphe]+xCmtoPixel(x3),yRef[graphe]+yCmtoPixel(y3),2*r3/(Xmax-Xmin)*subWidth[graphe],2*r3/(Ymax-Ymin)*subHeight[graphe]); // cercle C3

    // tracé de P3,P
    angle0=angle0-angle; // point précédent
    angle=angleGamma; // utilise AngleGamma

    xCalc=xLinePolaireRad(x3,y3,r3,angle0,angle);//xLinePolaireRad(xo,yo,r,angle0,angle)
    yCalc=yLinePolaireRad(x3,y3,r3,angle0,angle);//xLinePolaireRad(xo,yo,r,angle0,angle)

    //println ("x Calc ="+xCalc+" : x="+ x);
    //println ("y Calc ="+yCalc+" : y="+ y);

    stroke(vert);
    ellipse(xRef[graphe]+xCmtoPixel(xCalc),yRef[graphe]+yCmtoPixel(yCalc),5,5); // re-P

    stroke(rouge);
    strokeWeight(10); // largeur pourtour
    line(xRef[graphe]+xCmtoPixel(x3),yRef[graphe]+yCmtoPixel(y3),xRef[graphe]+xCmtoPixel(xCalc),yRef[graphe]+yCmtoPixel(yCalc));// r3
    strokeWeight(1); // largeur pourtour


    fill(noir);
    stroke(noir);
    rect(xRef[video],yRef[video],subWidth[video],subHeight[video]); // trace rectangle pour effacer sous fenêtre

} //=========== fin de la fonction de tracé des lignes du bras robotisé ================

//------------ fonction théorème de Al-Kashi pour calcul angle à partir 3 côtés ---------------------

/*

Cette fonction renvoie la valeur d'un angle d'un triangle quelconque connaissant
la longueur des 3 côtés du triangle, à savoir des 2 cotés adjacents et du coté opposé à cet angle.

Cette fonction applique le theoreme de Al-Kashi (ou théorème de Pythagore élargi)
Voir notamment : http://fr.wikipedia.org/wiki/Th%C3%A9or%C3%A8me_d%27Al-Kashi

Formule du theoreme de Al-Kashi :

                  (adj1² + adj2² - opp²)
angle =  acos   ( ------------------------- )
                  (2 x adj1 x adj2)

*/


float calculAngleRadAlKashi( float adj1, float adj2, float opp) {
// avec :
// adj1 et adj2 la longueur des 2 cotés adjacents à l'angle à calculer
//opp la longueur du coté opposé à l'angle à calculer

 //----------- variables utilisées par la fonction
 float D; // variable calcul dénominateur
 float N;  // variable calcul numérateur
 float calculAngleRad; // variable angle à calculer

 //----------- calcul du dénominateur : (adj1² + adj2² - opp²) ---------

 D=pow(adj1,2) + pow (adj2,2) - pow(opp,2);


 //----------- calcul du numérateur :  (2 x adj1 x adj2) ---------------

 N=2*adj1*adj2;

 //-------------- calcul final de l'angle ----------------------------

 calculAngleRad=acos(D/N); // calcule l'angle en radians

 //----- renvoi de la valeur calculée ----
 return (calculAngleRad);

}


//---------------------- fin fonction théorème Al-Kashi calcul angle à partir 3 cotés ------------------

//------------ fonction théorème de Al-Kashi pour calcul 3eme coté à partir 2 côtés et un angle (en radian) ---------------------

/*

Cette fonction renvoie la valeur du 3ème côté d'un triangle quelconque connaissant
l'angle opposé à ce 3ème côté
et la longueur des 2 autres côtés du triangle, à savoir des 2 cotés adjacents à l'angle utilisé.

Cette fonction applique le theoreme de Al-Kashi (ou théorème de Pythagore élargi)
Voir notamment : http://fr.wikipedia.org/wiki/Th%C3%A9or%C3%A8me_d%27Al-Kashi

Formule du theoreme de Al-Kashi :

opp=sqrt(adj1² + adj2² - (2 x adj1 x adj2 x cos (angle)))

*/


float calculCoteRadAlKashi( float adj1, float adj2, float angleOpp) {
// avec :
// angle est l'angle opposé au 3ème coté
// adj1 et adj2 la longueur des 2 cotés adjacents à l'angle utilisé

 //----------- variables utilisées par la fonction
 float D1; // variable calcul partiel
 float D2;  // variable calcul partiel
 float calculCote; // variable 3ème Coté à caclculer

 //----------- calcul partiel  adj1² + adj2² ---------

 D1=pow(adj1,2) + pow (adj2,2);


 //----------- calcul partiel  (2 x adj1 x adj2 x cos (angle)) ---------------

 D2=2*adj1*adj2*cos(angleOpp);

 //-------------- calcul final du coté ----------------------------

 calculCote=sqrt(D1-D2); // calcule le 3ème coté

 //----- renvoi de la valeur calculée ----
 return (calculCote);

}


//---------------------- fin fonction théorème Al-Kashi pour calcul 3ème coté ------------------

//------- fonction xLinePolaireRad(xo,yo,r,angle0,angle) ------
// renvoie x du point destination d'un segment connaisant longueur et angles en radians

float xLinePolaireRad(float xoIn,float yoIn,float rIn,float angle0In,float angleIn) {
// avec :  
// xo et yo : les coordonnées du point Po de départ du tracé
// r :  la longueur du segment à tracer
// angle0 : l'angle entre le segment passant par xo,yo et l'horizontale. en radians
// angle : l'angle à appliquer pour tracer le nouveau point P(x,y). en radians

float xOut; // variable de calcul

xOut=xoIn+(rIn*cos(angle0In-angleIn));

return (xOut);

}

//------- fonction yLinePolaireRad(xo,yo,r,angle0,angle) ------
// renvoie x du point destination d'un segment connaisant longueur et angles en radians

float yLinePolaireRad(float xoIn,float yoIn,float rIn,float angle0In,float angleIn) {
// avec :  
// xo et yo : les coordonnées du point Po de départ du tracé
// r :  la longueur du segment à tracer
// angle0 : l'angle entre le segment passant par xo,yo et l'horizontale. en radians
// angle : l'angle à appliquer pour tracer le nouveau point P(x,y). en radians

float yOut; // variable de calcul

yOut=yoIn+(rIn*sin(angle0In-angleIn));

return (yOut);

} // fin yLinePolaireRad


// Fonctions de gestion des évènements de la souris

void mousePressed() { // fonction appelée si bouton souris appuyé

        if ((mouseButton == LEFT)&&(mouseY>subHeight[graphe]-yRef[graphe]-50)&&(mouseX>xRef[graphe])) { // test si appui bouton gauche

                println("Clic bouton gauche !");

                println("X souris brut ="+ mouseX);

                xCalc=xPixeltoCm(mouseX-xRef[graphe]);
                println("xCalc ="+ xCalc);

                Rz=xCalc; // redéfinit l'abscisse du point cible P dans le plan x,y -- donc Rz au lieu de x

                if (Rz<decalageXBras+espaceMortX) Rz=decalageXBras+espaceMortX; // limite inférieure
                if (Rz>decalageXBras+espaceMortX+zoneActiveX) Rz=decalageXBras+espaceMortX+zoneActiveX; // limite sup

                //---- MAJ des variables globales de position du bras robotisé ---

                x=sqrt(pow(Rz,2)-pow(z,2)); // recalcule x d'après Rz et z dans le plan x,z

                calculAngles(x, y,z); // calcul des variables globales de position du bras robotisé à partir coordonnées du point cible

                //---- MAJ du tracé du bras robotisé à partir des variables globales de position ---
                background(blanc); // efface fenêtre graphique
                traceBras(); // tracé initial du Bras robotisé 3 segments

                //---- envoi instruction positionnement sur port Série vers Arduino

                positionneServos();

        }

        if (mouseButton == RIGHT) { // test si appui bouton droit
                println("Clic droit !");
        }


}

void mouseDragged() { // fonction appelée si appui sur un bouton et souris se déplace

/*
        if (mouseButton == LEFT) { // test si appui bouton gauche

                println("Déplacement souris + appui bouton gauche !");

                println("X souris brut ="+ mouseX);

                xCalc=map(mouseX,xRef[graphe],subWidth[graphe],Xmin,Xmax);
                println("X souris brut ="+ xCalc);

                x=xCalc; // redéfinit l'abscisse du point cible P

                if (x<decalageXBras+espaceMortX) x=decalageXBras+espaceMortX; // limite inférieure
                if (x>decalageXBras+espaceMortX+zoneActiveX) x=decalageXBras+espaceMortX+zoneActiveX; // limite sup

                //---- MAJ des variables globales de position du bras robotisé ---

                calculAngles(x, y , z); // calcul des variables globales de position du bras robotisé à partir coordonnées du point cible

                //---- MAJ du tracé du bras robotisé à partir des variables globales de position ---
                background(blanc); // efface fenêtre graphique
                traceBras(); // tracé initial du Bras robotisé 3 segments

                //---- envoi instruction positionnement sur port Série vers Arduino

                positionneServos();
                delay(250);


        }

        if (mouseButton == RIGHT) { // test si appui bouton droit
                println("Clic droit !");
        }
*/


}

void mouseReleased() { // fonction appelée si bouton souris relaché

}

void mouseClicked() { // fonction appelée si bouton souris enfoncé puis relâché


}

void mouseMoved() { //fonction appelée si la souris se déplace sans être appuyée

}


//---------------------------- fonction positionne Servos -----
//---- envoi instruction positionnement sur port Série vers Arduino

void positionneServos() { // utilise la valeur courante de l'angle Theta pour la base tournante

                //  myPort.write('H'); // envoie le caractère sur le port Série
                // myPort.write("chaine\n"); // envoie la chaine suivi saut ligne sur le port Série


                // ---- calcul des angles réel des servomoteur pour envoi vers Arduino                
                angleServo[0]=int(float(angleBaseFace)+degrees(angleTheta)); // servo base tournante à angleFace+angleTheta
                println("angle Servo 0 ="+angleServo[0]);

                angleServo[1]=int(degrees(angleAlpha-HALF_PI)); // angle servo 1 (point P1) - vertical à 90°
                println("angle Servo 1 ="+angleServo[1]);


                angleServo[2]=int(degrees(HALF_PI-angleBeta)); // angle servo 2 (point P2) - aligné à 90°
                println("angle Servo 2 ="+angleServo[2]);

                angleServo[3]=int(degrees(HALF_PI-angleGamma)); // angle servo 3 (point P3) - aligné à 90°
                println("angle Servo 3 ="+angleServo[3]);

                angleServo[4]=45; // servo pince à 45° = ouverte
                println("angle Servo 4 ="+angleServo[4]);

                //Envoi des valeurs sous forme de chaine de caractère au format xxx                

                String[] subStringOut=new String[5]; // crée tableau pour envoi des chaines

                for (int i=0; i<5; i++){

                    if (angleServo[i]<10) subStringOut[i]="00"+angleServo[i]; // format 00x
                    else if ((angleServo[i]>=10) && (angleServo[i]<100))subStringOut[i]="0"+angleServo[i]; // format 0xx
                    else subStringOut[i]=""+angleServo[i]; // format xxx

                    println ("chaine "+i+" ="+subStringOut[i]);          
                }

                if (positionInitiale==false) myPort.write("servotoR(001,015)\n"); // relève un peu le bras avant positionnement si on est pas en position initiale

                delay(250);

                myPort.write("servosBrasSync("+subStringOut[0]+","+subStringOut[1]+","+subStringOut[2]+","+subStringOut[3]+","+subStringOut[4]+")\n"); // envoie la chaine suivi saut ligne sur le port Série

                positionInitiale=false; // drapeau de position initiale

                //servosBrasSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle absolu

} // fin de la fonction positionneServos
/*
//------------ gestion évènement clavier ---------

void keyPressed() { // si une touche est appuyée

        if(key=='a') { // si touche a enfoncee

        }

        if (key == CODED) { // si appui d'une touche particulière (les touches spéciales up, down, etc.. cf Keycode)
                if (keyCode == UP) { // si touche Haut appuyée
                } else if (keyCode == DOWN) {// si touche BAS appuyée
                }

        } //--- fin si appui touche spéciale --

} //--- fin si touche enfoncee

*/



//---- evenement bouton goto

void gotoButton(int theValue) { // fonction évènement Button de meme nom - reçoit la valeur

        println("Evènement gotoButton avec valeur = "+theValue);

        if (xpositionText.getText()!=null) { // si le champ n'est pas vide

                xCalc = float(xpositionText.getText()); // récupère la valeur depuis le champ texte
                println("La position x est : "+xCalc);

        }

        if (zpositionText.getText()!=null) { // si le champ n'est pas vide

                zCalc = float(zpositionText.getText()); // récupère la valeur depuis le champ texte
                println("La position z est : "+zCalc);


        }

        brasGotoX(xCalc+decalageXBras, zCalc); // lance positionnement du bras à la position voulue - angle Theta courant sera utilisé pour la base

} // fin evènement bouton goto

//---- evenement bouton ramasse

void ramasseButton(int theValue) { // fonction évènement Button de meme nom - reçoit la valeur

        println("Evènement ramasseButton avec valeur = "+theValue);

  ramasseBalle(); // appel fonction

} // fin evènement bouton ramasse

//--- fonction goto + Ramasse ---
void gotoRamasseButton(int theValue) { // fonction évènement Button de meme nom - reçoit la valeur

        println("Evènement gotoRamasseButton avec valeur = "+theValue);

        if (xpositionText.getText()!=null) { // si le champ n'est pas vide

                xCalc = float(xpositionText.getText()); // récupère la valeur depuis le champ texte
                println("La position x est : "+xCalc);

        }

        if (zpositionText.getText()!=null) { // si le champ n'est pas vide

                zCalc = float(zpositionText.getText()); // récupère la valeur depuis le champ texte
                println("La position z est : "+zCalc);


        }

        brasGotoX(xCalc+decalageXBras, zCalc); // lance positionnement du bras à la position voulue - angle Theta courant sera utilisé pour la base

        ramasseBalle(); // appel fonction

} // fin evènement bouton goto + ramasse

//------------------- fonction bras Goto X ---------------

void brasGotoX( float xIn, float zIn) {

                 x=xIn;//+espaceMort; // redéfinit l'abscisse du point cible P
                 z=zIn; // utilise valeur reçue pour z  

                if (x<decalageXBras+espaceMortX) x=decalageXBras+espaceMortX;//+espaceMortX; // limite inférieure
                if (x>decalageXBras+espaceMortX+zoneActiveX) x=decalageXBras+zoneActiveX+espaceMortX;//+espaceMortX; // limite sup

                //---- MAJ des variables globales de position du bras robotisé ---

                calculAngles(x, y, z); // calcul des variables globales de position du bras robotisé à partir coordonnées du point cible

                //---- MAJ du tracé du bras robotisé à partir des variables globales de position ---
                background(blanc); // efface fenêtre graphique
                traceBras(); // tracé initial du Bras robotisé 3 segments

                //---- envoi instruction positionnement sur port Série vers Arduino

                positionneServos();

}

//----------------- fonction pour ramasser balle à partir de la position actuelle du bras -------------
void ramasseBalle() { // fonction ramasse balle


        myPort.write("servoto(004,080)\n"); // envoie la chaine suivi saut ligne sur le port Série - ferme pince
        delay(500);

        //soulève bras et rotation - pince fermée
        myPort.write("servosBrasSync("+angleBaseRamasse+",090,010,010,080)\n"); // envoie la chaine suivi saut ligne sur le port Série
        delay(1000);

        //baisse bras
        myPort.write("servosBrasSync("+angleBaseRamasse+",070,000,000,080)\n"); // envoie la chaine suivi saut ligne sur le port Série
        delay(1000);

        myPort.write("servoto(004,000)\n"); // envoie la chaine suivi saut ligne sur le port Série - ouvre pince
        delay(500);

        myPort.write("servosBrasSync("+angleBaseFace+",120,010,010,085)\n"); // position initiale pince fermée
        delay(1000);

        positionInitiale=true; // drapeau de position initiale
}

//===================== fonction analyse image webcam ===============================

// cette fonction assure la reconnaissance visuelle de la balle et extrait les coordonnées de la balle dans la zone active ---
void analyseWebcam() {

        opencv.read(); // lecture flux vidéo via OpenCV

        img1=opencv.image(); // récupère Image opencv dans Processing

        traceBras(); // tracé du Bras robotisé 3 segments  


        //traitement de l'image de capture openCV dans Processing

        image( img1, xRef[video], yRef[video] );   // affichage image video

        //----- 1°) application du "mixeur de canaux" avec sortie sur canal Rouge
            //---- coeff à appliquer
            float coefRouge=1; // 100% de rouge
            float coefVert=1.5; // 80% du vert
            float coefBleu=-2; // - 200% du bleu
            //---------- le traitement le plus efficace qui fonctionne est à tester dans Gimp au préalable
            //--------- ici réglage pour balle de couleur orangée ----

          img1.loadPixels(); // charge les pixels de l'image

          for (int i = 0; i < subWidth[video]*subHeight[video]; i++) { // passe en revue les pixels de l'image - index 0 en premier

            float r = (red(img1.pixels[i])) + (green(img1.pixels[i])*coefVert) + (blue(img1.pixels[i])*coefBleu); // la couleur rouge
            //---- fonction mixeur de canaux
            //---- le canal rouge est le canal de sortie et a pour coeff 1
            //---- auquel on ajoute du vert avec coeff vert
            //---- et du bleu avec coeff bleu

            // les deux autres canaux restent inchangés
            float g = green(img1.pixels[i]); // la couleur verte

            float b = blue(img1.pixels[i]); // la couleur bleue

             img1.pixels[i] = color(r, g, b); // modifie le pixel en fonction

            }

          img1.updatePixels();  // met à jour les pixels  

        //----- 2°) transformation de l'image en monochrome en se basant sur le canal rouge

          img1.loadPixels(); // charge les pixels de la fenetre d'affichage

          for (int i = 0; i < subWidth[video]*subHeight[video]; i++) { // passe en revue les pixels de l'image - index 0 en premier

            float r = red(img1.pixels[i]);// la couleur rouge
            float g = red(img1.pixels[i]); // la couleur verte

            float b = red(img1.pixels[i]); // la couleur bleue

             img1.pixels[i] = color(r, g, b); // modifie le pixel en fonction

            }

          img1.updatePixels();  // met à jour les pixels  

        //------ on applique filtre de seuillage ---
        img1.filter(THRESHOLD,0.7); // applique filtre seuil à la fenetre d'affichage
        // à adapter en fonction de la luminosité ambiante - plus sombre, valeur 0.7, plus lumineux, valeur 0.8 ou plus  

        //--- on récupère l'image transformée ---
        //img2=get(0,0,subWidth[video],subHeight[video]); // récupère image à partir fenetre d'affichage


        //--- on rebascule dans OpenCV ---
        opencv.copy(img1); // charge l'image modifiée dans le buffer opencv

        // trouve les formes à l'aide de la librairie openCV
        // blobs(minArea, maxArea, maxBlobs, findHoles, [maxVertices]);
        Blob[] blobs = opencv.blobs( 10, subWidth[video]*subHeight[video]/4, 5, false, OpenCV.MAX_VERTICES*4 );


        // ---- recharge image vidéo non traitée ---
        opencv.read(); // lecture flux vidéo via OpenCV
        noTint();
        image( opencv.image(), 0, 0 );   // affichage image video


       // xxxxxxxxxxxx Analyse et gestion des formes reconnues xxxxxxxxxxxxxx
      for( int i=0; i<blobs.length; i++ ) { // passe en revue les blobs (= formes détectées)

      //---- détection du "centre" de l'objet ----
      int centreX= blobs[i].centroid.x; // centroid renvoie un objet Point qui fournit x et y
      int centreY= blobs[i].centroid.y; // centroid renvoie un objet Point qui fournit x et y

      /*
      //---------- dessine un cercle autour du centre détecté -----------
      noFill();
      stroke(vert);
      strokeWeight(2);
      ellipse (centreX,centreY, 10,10);
      */


       Rectangle rectangleBlob=blobs[i].rectangle; // récupère le rectangle qui contient la forme détectée

       //---- analyse du rectangle objet ----
       int ratioWH=rectangleBlob.width/rectangleBlob.width; // calcule le ratio largeur/hauteur
       int ratioHW=rectangleBlob.height/rectangleBlob.height; // calcule le ratio hauteur/largeur

       float aireBlob=blobs[i].area; // récupère l'aire de la forme courante


       float ratioAire=aireBlob/(rectangleBlob.width*rectangleBlob.height); // calcul du ratio Aire forme / Aire rectangle
       // aire cercle = pi * r²
       // aire carré = (2r)²=4r²
       // aire cercle/aire carré = (pi * r²)/ (4*r²) = pi/4 = 3/4 env
       // un cercle occupe 3/4 du carré le contenant

       //if ((ratioWH>0.9)) { // si forme proche du carré
       if ((ratioAire>0.5) && ((ratioWH>0.8) || (ratioHW>0.8))) { // si le rapport de l'aire de la forme / rectangle contenant est compatible avec un cercle

        //fill(jaune); // couleur remplissage RGB
        noFill(); // pas de remplissage
        stroke (bleuclair); // couleur pourtour RGB

        //tracé d'un rectangle autour du blob
        rect( rectangleBlob.x, rectangleBlob.y, rectangleBlob.width, rectangleBlob.height );

        fill(jaune); // couleur remplissage RGB
        stroke (rouge); // couleur pourtour RGB


        // tracé des formes détectées
        beginShape(); // début tracé forme complexe

        for( int j=0; j<blobs[i].points.length; j++ ) { // parcourt tous les points du pourtour du blob
            vertex( blobs[i].points[j].x, blobs[i].points[j].y ); // tracé des points de la forme
        }
        endShape(CLOSE); // tracé forme complexe



        //--------------- gestion centre pour servomoteur ----------
        // moyenne des X et des Y

        moyX=moyX+centreX;
        moyY=moyY+centreY;
        comptImg=comptImg+1;

        if (comptImg>=nbImg) { // si le nombre d'image à prendre en compte dépassé

           //println("X = "+ centreX);
           //println("Total X = "+ moyX);
           //println("comptImg="+comptImg);        
           moyX=moyX/(comptImg); // calcule moyenne centre X
           println ("Moyenne X = "+ moyX);

           //println("Y = "+ centreY);
           //println("Total Y = "+ moyY);
           //println("comptImg="+comptImg);        
           moyY=moyY/(comptImg); // calcule moyenne centre X
           println ("Moyenne Y = "+ moyY);

           xCm=map(moyX,0,subWidth[video],0,largeurImageCm)-(largeurImageCm/2);
           println ("X Cm= "+ xCm);

           yCm=map(moyY,0,subHeight[video],0,hauteurImageCm)-(hauteurImageCm/2);
           println ("Y Cm= "+ yCm);

           //------------ attention : le Xcm Bras est basé sur le yCm de l'image ---
           xCmBras=espaceMortX+((tailleZoneUtileCm=18/2)-yCm);
           println ("X Cm Bras= "+ xCmBras);

           yCmBras=-xCm; // et le yCmBras sur le xCm

           println ("Y Cm Bras= "+ yCmBras);

           if (liveToggle.getState()==true) { // si mode live activé


             println("Mode live actif");
             xpositionText.setValue(Float.toString(xCmBras)); // prend en compte la valeur position balle détectée dans la zone active

             zpositionText.setValue(Float.toString(yCmBras)); // prend en compte la valeur position balle détectée

           }

           /*
           //--- positionnement servomoteur ---
           if (moyX<((width/2)-15)) { // si on est vers la droite

                  myPort.write("servoPantoR(002)\n");// on incrémente la position de l'angle du servomoteur
           }

           if (moyX>((width/2)+15)) { // si on est vers la droite

                  myPort.write("servoPantoR(-002)\n");// on incrémente la position de l'angle du servomoteur
           }

            */



        //--- dessine croix centrale centre balle
        stroke(bleu);
        line (moyX-10,moyY,moyX+10,moyY); // trait horizontal
        line (moyX,moyY-10,moyX,moyY+10); // trait vertical

           comptImg=0; // RAZ comptage images
           moyX=0; // RAZ moyX
           moyY=0; // RAZ moyY
        }



       }
       else { // trace avec couleurs différentes les rectangles non souhaités

 /*      
        fill(bleu); // couleur remplissage RGB
        stroke (jaune); // couleur pourtour RGB

        //tracé d'un rectangle autour du blob
        rect( rectangleBlob.x, rectangleBlob.y, rectangleBlob.width, rectangleBlob.height );

        fill(jaune); // couleur remplissage RGB
        stroke (rouge); // couleur pourtour RGB

        // tracé des formes détectées
        beginShape(); // début tracé forme complexe

        for( int j=0; j<blobs[i].points.length; j++ ) { // parcourt tous les points du pourtour du blob
            vertex( blobs[i].points[j].x, blobs[i].points[j].y ); // tracé des points de la forme
        }
        endShape(CLOSE); // tracé forme complexe
*/
   

       }







    } // ---- fin for blobs --------

        //--- dessine croix centrale
        stroke(rouge);
        line ((subWidth[video]/2)-tailleCroix,subHeight[video]/2,(subWidth[video]/2)+tailleCroix,subHeight[video]/2); // trait horizontal
        line (subWidth[video]/2,(subHeight[video]/2)-tailleCroix,subWidth[video]/2,(subHeight[video]/2)+tailleCroix); // trait vertical


        //---- dessine les points des angles par calcul ---

        stroke(jaune);
        fill(violet);

        for (int i=0; i<4; i++) {

                  ellipse (xPixelCoin[i],yPixelCoin[i],5,5);  // trace point      

        }        

}

//================= fin analyse webcam ===============

//XXXXXXXXXXXXXXXXXX Fin du programme XXXXXXXXXXXXXXXXX