+/**
+#*******************************************************************************
+# Projet : VOR-005
+# Sous projet : calibrateur de servo moteurs
+# Auteurs: Gille De Bussy
+# 
+# Program principal:
 
 // A FAIRE : gérer les vitesse : les mettre à fond quand test usb les mettre plus bas pour les autres modes.
+
+#*******************************************************************************
+ J.Soranzo
+	16/12/2015 gestion sous git + github
+	https://github.com/gilles293/calibrateurServo.git
+	nettoyage du code, j'ai craqué
+*/ 
+
 
 
 #include <SCoop.h>

          }

defineTimerRun(surveilleServo,100) //conserver 100 ou modifier le "*0.1" dans méthode appliqueobjectif du Servo
        {  
         //Serial.println("Servo");
         //Serial.println("potence.
         leServo.appliqueObjectif();
         potence.appliqueObjectif();
         

         
          }


defineTimerRun(surveillePotar,80)
  {
   // Serial.println("potar");
    lePotar.refresh();
             
    
    }

defineTask(reflechi)
      
      
      void reflechi::setup()
      
          {
             
           }
    
    void reflechi::loop()
          

//defineTimerRun(reflechi,40)
  {
    //Serial.println("proute");
    //Serial.println("reflechi");
      if (boutonP.hasBeenLongClicked())
        { 
          
          dateChangeServo=(millis());
          changeTypeServo=true;
          
          if (leServo.getType())
              {
                leServo.setType(false);
                affichage.affiche(5);
                Serial.println(F("servo de type Adafruit"));
                
                }
           else
             {
               leServo.setType(true);
               affichage.affiche(6);
               Serial.println(F("servo de type Classique"));
               }
          //sleep( 3000);
          boutonP.acquit();
          }
          
           
           if (millis()>dateChangeServo+3000 && changeTypeServo)
          {
           affichage.affiche(etatCalibrateur);
           changeTypeServo=false;
            }
                 
          
          
          
          
    int objTest (0);
    
    switch (etatCalibrateur)
        {
        
          
          
          
          case 1:
             
             Serial.println(F("Double Clic pour procéder à l'etlonnage"));
             etatCalibrateur=10;
             break;
             
             
                   
          case 10:
          
             if (boutonP.hasBeenClicked())
            {
              boutonP.acquit();
              etatCalibrateur=2;
              affichage.affiche(2);
              leServo.setObjectif(leServo.getMin());
                                          
              }
             
             if (boutonP.hasBeenDoubleClicked())
             {
               boutonP.acquit();
               leServo.setVitesse(4000);
               Serial.println(F("tourner potar à fond à gauche"));
               etatCalibrateur=11;
               
              } 
           
             break;   

          case 11:
              
              leServo.setObjectif(leServo.getMilieu());// pour etre certain qu'on aprt du milieu et que le servo n'est pas n'importe ou suite à des manip de type sweep ou potar par ex
              if (lePotar.getValue()>1000)
             {
               etatCalibrateur=12;
               Serial.println(F("quand potence reglé (sur le disque optique) faire 1 clic"));
                              
              }   
                 
             break;   
            
             
            
        case 12 :
   
//           Serial.print("obj potence=");
//           Serial.print(potence.getObjectif());
//           Serial.print("   Encours potence=");
//           Serial.print(potence.getEnCours());
//           Serial.print("   obj Pot=");
//           Serial.println(map(lePotar.getValue(),0,1023,potence.getMin(),potence.getMax()));
          // Serial.println("_____");
           
           
           potence.setObjectif(map(lePotar.getValue(),0,1023,potence.getMin(),potence.getMax()));
           if (boutonP.hasBeenClicked())
            {
              bonnePositionPotence=map(lePotar.getValue(),0,1023,potence.getMin(),potence.getMax());
              boutonP.acquit();
              etatCalibrateur=13;
              }
           
           
           
             break;
         
         case 13: //mesure des impulsion de reference sur une rotation de delta
             Serial.println("ICI");
             compteur=0;
             compteurRef=0;
             objTest=leServo.getMilieu()+DELTA;
             leServo.setObjectif(objTest);
            
             sleep(TEMPOSLEEP);
             etatCalibrateur=14;
             Serial.println("la");
             compteurRef=compteur;
             if (compteurRef==0)
               {
                 Serial.println(F("PB : compteur Ref=0 aucune impulsion fourche optique détecté"));
                 Serial.println(F("retour au début de procédure"));
                 etatCalibrateur=1;
               }

             
             break;
        case 14: 
             Serial.println("lalalal");
             
               
             //sequence pour aller au max puis au min et de déterminer les pwm Min et Max du servo
             while (compteur>compteurRef-ERREURCOMPTAGE)
               {
                 Serial.println("LAICI");
                 compteur=0;
                 objTest=objTest+DELTA;
                 leServo.setObjectif(objTest);
                 sleep(TEMPOSLEEP);
                 
                 }
             leServo.setMax(objTest-DELTA);
             leServo.setObjectif(leServo.getMilieu());
             sleep(TEMPOSLEEP);
             compteur=compteurRef;
             
             while (compteur>compteurRef-ERREURCOMPTAGE)
               {
                 Serial.println(F("LAICIlabas"));
                 compteur=0;
                 objTest=objTest-DELTA;
                 leServo.setObjectif(objTest);
                 sleep(TEMPOSLEEP);
                               
                 
                 }
             leServo.setMin(objTest+DELTA);
             leServo.setObjectif(objTest+DELTA);
             
             sleep(TEMPOSLEEP);
             
             
       //séquence pour mesurer une vitesse et amplitude moyenne (10 cycle d'aller et retour et enregistrement systématique des mesures)
            byte i;
            
             for (i=0;i<20;i++)
             
             {
               compteur=0;
             compteurRef=0;//compteur ref utilisé différement dans la suite
               tempsMesureVitesse=millis();
               leServo.setObjectif(leServo.getMax());
               sleep(200);
               while (compteur>compteurRef);
                 {
                   compteurRef=compteur;
                   sleep(200);
                   }
               resultatImpulsion[i]=compteur;
               resultatTemps[i]=millis()-tempsMesureVitesse-200;
               
                compteur=0;
             compteurRef=0;
               tempsMesureVitesse=millis();
               leServo.setObjectif(leServo.getMin());
               sleep(200);
               while (compteur>compteurRef);
                 {
                   compteurRef=compteur;
                   sleep(200);
                   }
               resultatImpulsion[i+1]=compteur;
               resultatTemps[i+1]=millis()-tempsMesureVitesse-200;
               
               
               }
               
             Serial.println(F("ATTENTION regarder  et noter le sens de rotation du premier mouvement "));
             Serial.println(F("si pret faire un clic "));
             etatCalibrateur=15;
              
              
              case 15:
                if (boutonP.hasBeenClicked())
                    {
                      boutonP.acquit();
                      etatCalibrateur=16;
                      leServo.setObjectif(leServo.getMax());
                      } 
              
             break;
             
             
             case 16: 
               
               Serial.println(F("Alors quelle sens de rotation ?") ); 
               Serial.print(resultatImpulsion[5]);
               Serial.print(F(" impulsions en "));
               Serial.println(resultatTemps[5]);
               Serial.println(F(" ms")); 
               //
               //
               //
               //
                // a faire calculer moyenne des 2 tableau et calculer ecart type
                //
                ///
                potence.setObjectif(potence.getMax());
                Serial.println(F("si nouveau servo : doubleClic, si changement mode clic"));
                etatCalibrateur=17;               
               break;
               
               case 17: 
                if (boutonP.hasBeenClicked())
                    {
                      boutonP.acquit();
                      etatCalibrateur=2;
                      affichage.affiche(2);
                      leServo.setObjectif(leServo.getMin());
                      } 
               if (boutonP.hasBeenDoubleClicked())
                {
                  boutonP.acquit();
                  etatCalibrateur=13;
                  leServo.setObjectif(leServo.getMilieu());
                  potence.setObjectif(bonnePositionPotence);
                  sleep(TEMPOSLEEP*3);
                  }    
                      
                   break ;    
                  
        case 2:
             if (leServo.isMin())
            {
              
              etatCalibrateur=20;
              leServo.setObjectif(leServo.getMax());
                                         
              }
              
              if (boutonP.hasBeenClicked())
            {
              boutonP.acquit();
              etatCalibrateur=3;
              affichage.affiche(3);
              }
                 
             break;
    
    case 20:
             if (leServo.isMax())
            {
              
              etatCalibrateur=2;
              leServo.setObjectif(leServo.getMin());
                                         
              }
               
               if (boutonP.hasBeenClicked())
            {
              boutonP.acquit();
              etatCalibrateur=3;
              affichage.affiche(3);
              }
               
               
                 
             break;
    
    
             
            
        case 3:
          int temp;
          int potarVal;
          potarVal=lePotar.getValue();
          //Serial.print("lepotar : ");
          //Serial.println(potarVal);
          temp=map(potarVal,0,1023,leServo.getMin(),leServo.getMax());
          
          
//          if (leServo.getType())
//          
//            {
//              temp=map(potarVal,0,1023,300,3000);
//            }
//          else
//            {
//             temp=map(potarVal,0,1023,100,1000);
//              }
              
           //Serial.print ("OBJECTIF=  ");
           //Serial.println(temp);   
           leServo.setObjectif(temp);
           //potence.setObjectif(temp);

          if (boutonP.hasBeenClicked())
            {
              boutonP.acquit();
              etatCalibrateur=4;
              affichage.affiche(4);
              //leServo.setObjectif(leServo.getMilieu());
              
              }
          
          
          
          break;
    
      case 4:
           
           leServo.setObjectif(leServo.getMilieu());
           if (boutonP.hasBeenClicked())
            {
              boutonP.acquit();
              etatCalibrateur=1;
              affichage.affiche(1);
                            
              }
           
           
           
           
           break;
           
      case 8:
        
        break;
        
        
        
      }

    
    
    
    }


          



defineTimerRun(rafraichirAffichage,800) //ne pas mettre ce timer a une valeur trop faible pour éviter le rebond lors de l'apuui bouton (ne pas compter des apuis multiple pour un seul appuie voulu)   
{  
	 //Serial.println("affichagePP");
	  affichage.refreshAfficheur();
}

defineTimerRun(surveilleBouton,40)
//ne pas mettre ce timer a une valeur trop faible pour éviter le rebond lors de
//l'apuui bouton (ne pas compter des apuis multiple pour un seul appuie voulu)   
        {  
         //Serial.println("bouton");
          boutonP.refreshBouton();
