/////////////// 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

/*

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/

// 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

Textfield positionText; // déclare des objets Textfield

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

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

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

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 —
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
float x=26.0, y=0.0; // coordonnées du point P(x,y) – point central de la pince – connu = position de l’objet – sur axe des abscisses

//— 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

//— 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

//— 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é =============

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

//—— 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);

// 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(640, 480); // ouvre une fenêtre xpixels  x ypixels
        background(blanc); // couleur fond fenetre

// — 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,150,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,200,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

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

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

//—— 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

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

traceBras(); // 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

// tout se passe dans les fonctions évènements souris et bouton ControlP5

} // 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,width);
  return (xOut);

}

//—– Conversion yCmtoPixel() —–

float yCmtoPixel(float yIn) {

  float yOut;
  yOut=map(yIn,Ymin,Ymax,0,height);
  yOut=height–yOut;
  return (yOut);

}

//—– Conversion xPixeltoCm() —–

float xPixeltoCm(float xIn) {

  float xOut;
  xOut=map(xIn,0,width,Xmin,Xmax);
  return (xOut);

}

//—– Conversion yPixeltoCm() —–

float yPixeltoCm(float yIn) {

  float yOut;
  yIn=height+yIn;
  yOut=map(yIn,0,height,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) ================

// 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) – 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
float x=26.0, y=0.0; // coordonnées du point P(x,y) – point central de la pince – connu = position de l’objet – sur axe des abscisses

//— 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

//— 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

//— 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) { // 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)

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

//—– calcul de deltaX et deltaY —-

deltaX=x–x1;
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

    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(xCmtoPixel(0),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(xCmtoPixel(x1),yCmtoPixel(y1),5,5); // P1

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

    // cercle C1
    ellipse(map(x1,Xmin,Xmax,0,width),height–map(y1,Ymin,Ymax,0,height),2*r1/(Xmax–Xmin)*width,2*r1/(Ymax–Ymin)*height); // cercle C1

    // point P(x,y)
    ellipse(map(x,Xmin,Xmax,0,width),height–map(y,Ymin,Ymax,0,height),5,5); // P

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

    line(xCmtoPixel(0+decalageXBras+espaceMortX),yCmtoPixel(0),xCmtoPixel(0+decalageXBras+espaceMortX+zoneActiveX),yCmtoPixel(0)); // Zone active

    //—- affichage des positions tous les cm
    for (int i=0; i<=zoneActiveX; i++) {
      ellipse(xCmtoPixel(i+decalageXBras+espaceMortX),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(map(x2,Xmin,Xmax,0,width),height–map(y2,Ymin,Ymax,0,height),5,5); // P2

    stroke(bleu);
    strokeWeight(10); // largeur pourtour
    line(map(x1,Xmin,Xmax,0,width),height–map(y1,Ymin,Ymax,0,height),map(x2,Xmin,Xmax,0,width),height–map(y2,Ymin,Ymax,0,height));
    strokeWeight(1); // largeur pourtour

    stroke(violet);
    ellipse(map(x2,Xmin,Xmax,0,width),height–map(y2,Ymin,Ymax,0,height),2*r2/(Xmax–Xmin)*width,2*r2/(Ymax–Ymin)*height); // 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(map(x3,Xmin,Xmax,0,width),height–map(y3,Ymin,Ymax,0,height),5,5); // P3

    stroke(violet);
    strokeWeight(10); // largeur pourtour
    line(map(x2,Xmin,Xmax,0,width),height–map(y2,Ymin,Ymax,0,height),map(x3,Xmin,Xmax,0,width),height–map(y3,Ymin,Ymax,0,height)); // r2
    strokeWeight(1); // largeur pourtour

    stroke(rouge);
    ellipse(map(x3,Xmin,Xmax,0,width),height–map(y3,Ymin,Ymax,0,height),2*r3/(Xmax–Xmin)*width,2*r3/(Ymax–Ymin)*height); // 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(map(xCalc,Xmin,Xmax,0,width),height–map(yCalc,Ymin,Ymax,0,height),5,5); // re-P

    stroke(rouge);
    strokeWeight(10); // largeur pourtour
    line(map(x3,Xmin,Xmax,0,width),height–map(y3,Ymin,Ymax,0,height),map(xCalc,Xmin,Xmax,0,width),height–map(yCalc,Ymin,Ymax,0,height));// r3
    strokeWeight(1); // largeur pourtour

} //=========== 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>height–50)) { // test si appui bouton gauche

                println(« Clic bouton gauche ! »);

                println(« X souris brut = »+ mouseX);

                xCalc=map(mouseX,0,width,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); // 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

}

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() {

                //  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(angleBaseFace); // servo base tournante à 90°
                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 (positionText.getText()!=null) { // si un chemin fichier est sélectionné

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

                brasGotoX(xCalc); // lance positionnement du bras à la position voulue

        }

} // fin evènement bouton goto

//—- evenement bouton goto

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 goto

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

void brasGotoX( float xIn) { // reçoit la distance du point P au point O(0,0)

                 x=xIn;//+decalageXBras+espaceMort; // redéfinit l’abscisse du point cible P

                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); // 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
}

//XXXXXXXXXXXXXXXXXX Fin du programme XXXXXXXXXXXXXXXXX