// — 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 : 12/07/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 ? ———
 /*Un servomoteurs connectés à la carte Arduino est contrôlé
à partir du PC (interface Processing)

Controle par chaine de la forme :
* servoPan(000) avec 000 = angle absolu en degrés pour un positionnement immédiat
* servoPanto(000) avec 000 = angle absolu en degrés pour un positionnement progressif avec delai vitesse
* servoPantoR(000) avec 000 = angle relatif en degrés pour un positionnement progressif avec delai vitesse
* vitesse(000) avec 000 = delai en millisecondes à utiliser entre 2 positions intermédiaires (10 = rapide, 50 = lent)

*/

//cette version du programme vise à optimiser au maximum la vitesse de réactivité des servomoteurs

// — Fonctionnalités utilisées —

// Utilise / fonctionne avec une interface Processing coté PC
// Utilise 2 servomoteurs

// ——– Circuit à réaliser ———

// ******* ATTENTION : il est possible de connecter directement 2 ou 3 servomoteurs sur la carte Arduino
// Connecter un servomoteur PAN sur la broche 8
// Connecter un servomoteur TILT sur la broche 9

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

// — 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 :
//https://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 broche_servoPan=8; // Constante pour la broche 8
const int broche_servoTilt=9; // Constante pour la broche 9

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

// — Déclaration des variables globales —

boolean debug=false; // mettre à true si message debug

 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

float angleServoPan=90; // variable de position du servo  Pan en degrés
float angleServoPan0=90; // variable de la dernière position du servo  Pan en degrés

float angleServoTilt=20; // variable de position du servo  Tilt en degrés
float angleServoTilt0=20; // variable de la dernière position du servo  Tilt en degrés

int vitesse=0; // variable utilisée pour délai entre 2 lecture port Série

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

//— Création objet servomoteur
Servo servoPan;  // crée un objet servo pour contrôler le servomoteur 1
Servo servoTilt;  // crée un objet servo pour contrôler le servomoteur 2

// ////////////////////////// 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
servoPan.attach(broche_servoPan);  // attache l’objet servo à la broche de commande du servomoteur Pan
// se positione à 90° par défaut

servoTilt.attach(broche_servoTilt);  // attache l’objet servo à la broche de commande du servomoteur Pan
// se positione à 90° par défaut

// ——- Broches en sorties numériques ——-  
 pinMode (broche_servoPan,OUTPUT); // Broche broche_servoPan configurée en sortie
 pinMode (broche_servoTilt,OUTPUT); // Broche broche_servoTilt configurée en sortie

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

// ——- Codes d’initialisation utile ——-  

servoPan.writeMicroseconds(angle(angleServoPan)); // crée impulsion à partir valeur angle – plus précis que write()

servoTilt.writeMicroseconds(angle(angleServoTilt)); // crée impulsion à partir valeur angle – plus précis que write()

Serial.println(« Arduino OK »); // debug

delay(200);

} // 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
                if (debug) Serial.print (« 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

//chaineReception= » »;

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

//delay(vitesse);

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

  //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 vitesse(xxx) ———–
  if (testInstruction(« vitesse(« )==true) { // si instruction reçue valide

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

  } // fin test vitesse(xxx)

  //================ instructions servo Pan  =========

  //————– test instruction servoPan(xxx) ———–
  if (testInstruction(« servoPan(« )==true) { // si instruction reçue valide

    servoPan.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

    angleServoPan0=param; // mémorise angle actuel

  } // fin test genouD(xxx)

  //————– test instruction servoPanto(xxx) ———– // positionnement progressif
  if (testInstruction(« servoPanto(« )==true) { // si instruction reçue valide

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servoPan, angleServoPan0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleServoPan0=param; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    if (debug) Serial.print(« angleServoPan0 = « ), Serial.println(angleServoPan0);

  } // fin test genouDto(xxx)

  //————– test instruction servoPantoR(xxx) ———– // positionnement progressif
  if (testInstruction(« servoPantoR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servoPan, angleServoPan0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleServoPan0=param+angleServoPan0; // met à jour l’angle courant servo  

    if (debug) Serial.print(« angleServoPan0 = « ), Serial.println(angleServoPan0);

  } // fin test servoPantoR(xxx)

  //================ instructions servo Tilt  =========

  //————– test instruction servoTilt(xxx) ———–
  if (testInstruction(« servoTilt(« )==true) { // si instruction reçue valide

    servoTilt.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

    angleServoTilt0=param; // mémorise angle actuel

  } // fin servoTilt(xxx)

  //————– test instruction servoTiltto(xxx) ———– // positionnement progressif
  if (testInstruction(« servoTiltto(« )==true) { // si instruction reçue valide

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servoTilt, angleServoTilt0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleServoTilt0=param; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    if (debug) Serial.print(« angleServoTilt0 = « ), Serial.println(angleServoTilt0);

  } // fin servoTiltto(xxx)

  //————– test instruction servoTilttoR(xxx) ———– // positionnement progressif
  if (testInstruction(« servoTilttoR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servoTilt, angleServoTilt0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleServoTilt0=param+angleServoTilt0; // met à jour l’angle courant servo  

    if (debug) Serial.print(« angleServoTilt0 = « ), Serial.println(angleServoTilt0);

  } // fin test servoTilttoR(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

    if (debug) Serial.print(« Arduino va executer : « );

    if (debug) 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

      if (debug) Serial.println(« ) »); // affiche
      if (debug) Serial.println(« Instruction valide ! »); // affiche
      return(true); // renvoie true si instruction valide

    } // fin si fermeture parenthèse

    else {

      if (debug) Serial.println(« 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 ————

// ———- 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(« 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()
        if (debug) 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()
        if (debug) 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(« 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()
        if (debug)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()
        if (debug) Serial.print(« angle courant servo = « ), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin else

 }

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