 /****************************************************************
 *                Ecriture série des valeurs servos              *
 ****************************************************************/
- 
+        
+        
+        //SERVO 
 #include <Servo.h>
-
 int i=-1;
 int y=0;
-const int nbrServos=5;                              //Nombre de servomoteurs
-const int brocheServo[nbrServos]={3,5,7,8,12};   //Choix des broches pour les servomoteurs 3 5 6 9 10 11
+const int nbrServos=6;                              //Nombre de servomoteurs
+const int brocheServo[nbrServos]={3,5,6,9,13,11};   //Choix des broches pour les servomoteurs 3 5 6 9 10 11
 //const int brocheServo[nbrServos]={8,9};
 int angle[nbrServos];
-const int angle0[nbrServos]={90,90,95,180,90}; //90 0 180 180 90
+const int angle0[nbrServos]={90,180,0,90,90,100}; //90 0 180 180 90
 //int angle0[nbrServos]={90,90};
 Servo servo[nbrServos];
 
-//SERVO 2 : angle+ = montée : OK
-//SERVO 3 : angle+ = montée : ANGLE A INVERSER !!
+//Servo 2 : angle+ = montée : OK
+//Servo 3 : angle+ = montée : ANGLE A INVERSER !!
 //          Départ:180 degrés
-//SERVO 4 : angle+ = montée
+//Servo 4 : angle+ = montée
+
+
+        //VOLTMETRE
+int batteryLevel;
+float voltage;
+int mesure_voltage;
+  //R1=97.9 kOhms 
+  //R2=98.8 kOhms
+float periode=0; //permet de relentir la cadence d'envoi de la tension
+
+int pinLED=12;
 
 
 
 void setup() {
   Serial.begin(9600);
+  voltmetre();
+  Serial.print("Voltage = ");Serial.print(voltage);Serial.println(" V ");Serial.println();
+  
+  
+  if (voltage<6.5) 
+  {
+    Serial.println("CRITICAL ERROR : BATTERY TOO LOW !");
+    while(1);
+  }
   
   for (int j=0 ; j<2 ; j++){
     servo[j].attach(brocheServo[j], 800, 2100);     //Necessité de reduire la largeur de la PWM pour la restreindre (course des premiers servos~140)

         Serial.println(" ");
         i=0; 
      }
      else {
         //Serial.print(c);
         //Serial.print(" ");
         if(i>=0 && i<nbrServos) {
            angle[i]=c;
            //Serial.print("Servo numero ");
            Serial.print(i+1);

            i=-1; 
         }
      }
  
  }
  
  periode++;
 }
 
 
void voltmetre(){
    mesure_voltage = analogRead(0);
    voltage = mesure_voltage / (255/5);
    voltage = voltage * 0.50229;
    batteryLevel=(voltage-6.6)*100/2.4;
    if(batteryLevel > 0) Serial.println(batteryLevel);
    //Serial.print(voltage);
    //Serial.println(" V ");
    //else Serial.println("error");
}
 
 
void glowingLED(int periodeLED){
 for(int fadeValue = 0 ; fadeValue <= 255; fadeValue +=5) { 
    analogWrite(pinLED, fadeValue);         
    // wait for periodeLED milliseconds to see the dimming effect    
    delay(periodeLED);                            
  } 

  for(int fadeValue = 255 ; fadeValue >= 0; fadeValue -=5) { 
    analogWrite(pinLED, fadeValue);         
    // wait for periodeLED milliseconds to see the dimming effect    
    delay(periodeLED);                            
  } 
}
 
void flashingLED(int periodeLED){
  analogWrite(pinLED,250);
  delay(periodeLED);
  analogWrite(pinLED,0);
  delay(periodeLED);
}
 
void stillLED(){
  analogWrite(pinLED,250);
}
