-//Code permettant le fonctionnement du VAE. Il est implémenté sous Arduino Mega
//Code allowing the work of the electric bike. It's implemented on an Arduino Mega

//Occupation des pins sur l'arduino:
//Pins layout:
//Pins digitales:
//2,3,4,5,6,7 : écran (Screen)
//8 : controleur1 (Motor Driver 1)
//9 : controleur 2 (Motor Driver 2)
//22 : capteur TOR pédalier (Pedaling sensor 1)
//23 : capteur TOR pédalier (Pedaling sensor 2)
//24 : activation diver moteur (Motor driver activator 1)
//25 : activation diver moteur (Motor driver activator 2)
//26 : relais de sécurité (Safety relay : not implemented)
//27 : capteur TOR vitesse (Speed sensor)
//28 : relais éclairage (Light activator : not implemented)
//Pins analogiques: Analogic pins:
//A0 : potentiomètre de réglage d'assistance (Power selection potentiometer)
//A1 : capteur de courant (Current sensor)
//A2 : mesure tension batterie (Battery voltage sensor)
//A3 : mesure luminosité (Light intensity sensor : not implemented)

//Ecran a cristaux liquides Liquid cristal Screen:
#include <LiquidCrystal.h>
const int rs = 2, en = 3, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
//Affichages speciaux: Special characters:
//logo batterie0: index 0
byte customChar0[8] = {
  0b11111,
  0b10000,
  0b10101,
  0b10101,
  0b10101,
  0b10000,
  0b11111,
  0b00000
};
//logo batterie1: index 1
byte customChar1[8] = {
  0b11110,
  0b00010,
  0b01011,
  0b01001,
  0b01011,
  0b00010,
  0b11110,
  0b00000
};
//logo éclairage0 : index 2 //light1
byte customChar2[8] = {
  0b01111,
  0b11001,
  0b10001,
  0b10001,
  0b10001,
  0b11001,
  0b01111,
  0b00000
};
//logo éclairage1 : index 3 //light2
byte customChar3[8] = {
  0b00111,
  0b11000,
  0b00000,
  0b11111,
  0b00000,
  0b11000,
  0b00111,
  0b00000
};
//Affichage de l'écran : //Screen layout
//  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
//0 2 2 k m / h P : 0 0 0 W
//1 B B 1 5 %   A : 0 0 %   F F
//FF : indicateur d'allumage des feux //Light activation indicator



float puissance_servo = 0; //Variable comprise entre 0 et 1 donnant la puissance du moteur. //Variable between 0 and 1 giving the power of the motor/



//Capteur de courant //Current sensor
float gain_courant = 10.0*1.53; // en A/V
float U0 = 2.5; // en V

//Potentiometre réglage assistance //Power selection potentiometer
int consigne_pot = 0;

//capteur de pédalage
boolean is_turning = false; //variable indiquant si le pédalier est en train de tourner ou pas. //Variable indicating if the pedals are turning or not
boolean actual_color = false; // variable indiquant sur quelle couleur le pédalier est actuellement. false pour noir, true pour blanc. //Variable indicating on which state the sensor is at the moment. 
unsigned long compteur; //compteur indiquant la date de changement de couleur //Variable indicating the color change date
unsigned long comparaison_compteur; //compteur récupérant à chaque passage de boucle la date. //Variable giving at each iteration of the loop the date at the moment

//Relais de sécurité //Safety relay : not implemented
unsigned long date_ouverture_relais = 0; //compteur permettant de connaître la date d'ouverture du relais.
boolean can_be_opened = false; //variable décrivant si le relais peut être ouvert ou pas.

//Driver moteur //Motor driver
#include "BTS7960.h"
const uint8_t EN = 25; //2 pins enable, prennent les ports 25 et 24 sur la carte
const uint8_t L_PWM = 9; 
const uint8_t R_PWM = 8;
BTS7960 motorController(EN, L_PWM, R_PWM);

//Mesure de la vitesse //Speed measurement
float vitesse = 0.0;
float timer_speed = 0.0;
float delta_t = 0.0;
boolean magnet_visible = false;
float wheel_radius = 0.33; //TO BE CHANGED : the radius of your wheel

//Eclairage automatique //Automatic light
boolean is_light_activated = false;
int activate_light = 800;
int desactivate_light = 900;

//Données de l'asservissement //System control data
float max_current = 25;
float consigne = 0;
float mesure_courant[5] = {0,0,0,0,0};
float mesure_courant_copie[5] = {0,0,0,0,0};
float courant_filtre = 0; 
float gain_commande = 0.0003 ; //Gain de la loi de commande //Proportional gain
float gain_commande_max = 0.01;//Gain max du gain différentiel //not implemented
float gain_commande_min = 0.001;//Gain min du gain différentiel //not implemented
float sat_haut = 1; //Valeur haute de la saturation de l'intégrateur //integrator saturation higher value
float sat_bas = -1; //Valeur basse de la saturation de l'intégrateur //integrator saturation lower value
float compteur_integ = 0; //Valeur servant de pas à l'intégrateur //Step value for the integrator
float integ = 0; //Integrateur, avec valeur de base à 0. //Start value of the integrator

//Securité batterie
int seuil_secu = 675; //Seuil en dessous duquel la batterie se met en sécurité. //Minimal tension of the battery
boolean secu_batterie = false; // se met à true si jamais la sécurité devait s'activer. Est réinitialisée uniquement par redémarrage du système. //Gets to true if the battery safety is triggered.


void setup() {
  Serial.begin(9600);
  
  //PinOut:
  pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(22,INPUT);
  pinMode(23,INPUT);
  pinMode(26,OUTPUT);
  pinMode(27,INPUT);
  pinMode(28,OUTPUT);

  // set up the LCD
  lcd.begin(16, 2);
  //Creation des caractères personnalisés //Personalised character initialization
  lcd.createChar(0, customChar0); // create a new custom character batterie index 0
  lcd.createChar(1, customChar1); // create a new custom character batterie index 1
  lcd.createChar(2, customChar2); // create a new custom character batterie index 0
  lcd.createChar(3, customChar3); // create a new custom character batterie index 1
  //Affichage des écritures de base sur l'écran //Basis writings on the screen
  lcd.setCursor(0, 0); //colonne,ligne //column, line
  lcd.print("00km/h          ");
  lcd.setCursor(0, 1);
  lcd.write((byte)0);
  lcd.setCursor(1, 1);
  lcd.write((byte)1);
  lcd.setCursor(2, 1);
  lcd.print(get_capacity());
  lcd.print("%");
  lcd.setCursor(7, 0);
  lcd.print("P:");
  lcd.print(get_power());
  lcd.print("W");

  set_power(0);

  //Donne la bonne couleur à la variable actual_color pour éviter un démarrage intempestif à l'allumage du système.
  //Gives the start color to the variable actual_color.
  if (digitalRead(23) == HIGH){
    //Si la couleur détectée est blanche, alors on passe actual_color sur true.
    actual_color = false;
  }
  //initialisation du compteur
  //counter initialisation
  compteur = millis();
  //ouverture initiale du relais
  //relay initial opening
  digitalWrite(26,LOW);

  //Initialisation du capteur de courant*
  //Current sensor initialisation
  U0 = (float(analogRead(A1))/1024.0)*5.0;

  //Changement de la fréquence PWN à 3KHZ pour les pins 9 et 10
  //Change of frequency of the PWM
  TCCR2B = TCCR2B & B11111000 | B00000010;  // for  PWM frequency of 3921.16 Hz

  //Activation du moteur
  //Motor enabling
  motorController.Enable();

}

void loop() {

  
  //Partie du code permettant d'obtenir si le pédalier tourne ou pas.
  //Au pédalier est fixé une roue transparente avec 5 créneaux noirs et 5 blancs en alternance.
  //On a une fréquence de pédalage comprise entre 0.5tr/s et 3tr/s, soit une fréquence d'alternance de motifs comprise entre 5 et 30Hz. 
  //D'après le critère de Shanon, il faut échantillonner à au moins 60Hz. Par sécurité, on exigera que le programme boucle à au moins 100Hz.
  //Le fonctionnement est le suivant : à chaque changement de motifs, on remet le compteur à 0. Si pendant 200ms on n'a pas de changement(vitesse de pédalage inférieure à 0.5tr/s),
  //on met la variable booléenne is_turning à false.
  //Part of the code to obtain if the crankset turns or not.
  //To the pedalboard is fixed a transparent wheel with 5 black and 5 white slots in alternation.
  //We have a pedaling frequency between 0.5tr/s and 3tr/s, that is to say a frequency of alternating patterns between 5 and 30Hz. 
  // According to Shanon's criterion, we must sample at least 60Hz. For safety, we will require that the program loops at least 100Hz.
  //The operation is as follows: at each change of pattern, the counter is reset to 0. If for 200ms there is no change (pedaling speed lower than 0.5tr/s),
  //the boolean variable is_turning is set to false.
  
  if (digitalRead(23) == LOW && actual_color == false){ //Si la couleur mesurée est noire et que le programme croit que la couleur est blanche //If the measured color is black and the program thinks it's white
    //Le pédalier tourne à priori, on met compteur à la date actuelle
    //The crankset turns, we put counter to the current date
    compteur = millis();
    actual_color = true;//Mise à jour du paramètre //Update of the parameter
    //Serial.println("detect1");
  }
  if (digitalRead(23) == HIGH && actual_color == true){ //Si la couleur mesurée est blanche et que le programme croit que la couleur est noire //If the measured color is white and the program thinks it's black
    //Le pédalier tourne à priori, on met compteur à la date actuelle.
    //The crankset turns, we put counter to the current date
    compteur = millis();
    actual_color = false;//Mise à jour du paramètre //Update of the parameter
    //Serial.println("detect2");
  }
  //Ensuite, si ça fait moins de 200ms qu'on est sur cette couleur, on met is_turning à true. Sinon, on le met à false.
  //Then, if it's been less than 200ms that we are on this color, we set is_turning to true. Otherwise, we set it to false.
  comparaison_compteur = millis();
  if (comparaison_compteur - compteur < 300 && comparaison_compteur > 400){
    is_turning = true;
  }
  else{
    is_turning = false;
  }

  

  //Si le moteur n'est pas censé tourner, on ouvre le relais par mesure de sécurité, et on met le moteur à puissance 0
  //If the motor isn't supposed to be turning, we open the relay
  if (is_turning == false){
    digitalWrite(26,LOW);
    //set_power(0);
    puissance_servo = 0;
    integ = 0;
  }

  
  //Récupération et affichage de la valeur du potentiomètre de réglage de la puissance + Sécurités de vitesse et de batterie
  //Measuring the value of the potentiometer
  consigne_pot = analogRead(A0);
  //Si la consigne est en dessous de 5%, on considère que l'utilisateur veut éteindre le système. On met donc consigne_pot à 0, et on met le moteur en roue libre.
  //If the value is under 5%, we consider it as 0, and the motor is put into freewheel mode
  if (consigne_pot <= 51){
    consigne_pot = 0;
    motorController.Disable();
    is_turning = false;
  }
  //Si la vitesse est au dessus de 27km/h, alors on désactive le moteur et on met la consigne à 0
  //If the speed is over 27km/h, the motor is put into freewheel mode
  //Pour l'asservissement, on va lui faire croire que le pédalier n'est pas en train de tourner, ce qui va bloquer la boucle.
  //For the system control, we tell the program the crankset isn't working.
  if (vitesse > 27){
    consigne_pot = 0;
    motorController.Disable();
    is_turning = false;
  }
  //Si la tension de la batterie est inférieure à 5%, alors on coupe le moteur pour la protéger. DESACTIVE!!! //Not implemented
  if (analogRead(A2) < seuil_secu && is_turning == false && 0==1 ){
    secu_batterie = true;
    //Affichage de batterie vide
    lcd.setCursor(2, 1);
    lcd.print("STOP");
    Serial.println("secu batterie");
  }
  consigne = (float(consigne_pot)/1024.0)*float(max_current);


  //détermination du courant. On fait la moyenne sur les 6 dernières mesures.
  //Computing the current, average of the last 6 measurements.
  float courant_instant = get_Current();
  //Serial.println(analogRead(A1));
  courant_filtre = 0;
  for (int i=0; i<5; i++){
    courant_filtre += mesure_courant[i];
    mesure_courant_copie[i] = mesure_courant[i];
    if (i == 0){
      courant_filtre += courant_instant;
    }
  }
  mesure_courant[0] = courant_instant;
  mesure_courant[1] = mesure_courant_copie[0];
  mesure_courant[2] = mesure_courant_copie[1];
  mesure_courant[3] = mesure_courant_copie[2];
  mesure_courant[4] = mesure_courant_copie[3];
  
  courant_filtre = courant_filtre/6;

  //Si le moteur est censé tourner et que la consigne n'est pas à 0, on met en place l'asservissement. Sinon, on remet la puissance à 0 en attendant.
  //L'asservissement est codé avec les données du modèle simulink.
  //Si la consigne est à 0, on n'enclenche pas le processus afin de libérer le moteur.
  //On vérifie aussi que la batterie ne soit pas déchargée. Sinon , on libère le moteur.
  //If the motor is supposed to be running and the setpoint is not at 0, the slaving is set up. If not, the power is reset to 0 while waiting.
  //The control system is coded with the data of the simulink model.
  //If the setpoint is 0, we do not start the process in order to release the motor.
  //We also check that the battery is not discharged. Otherwise, we release the motor.
  if (is_turning == true && consigne != 0 && secu_batterie == false){
  //is_turning = true;
  //if (1 == 1){
    //Sommateur
    float ecart = consigne-courant_filtre;
    //Bloc de gain
    float gain_diff = (consigne/max_current)*(gain_commande_max-gain_commande_min) + gain_commande_min; //Not implemented
    ecart = ecart*gain_commande; //Ancien système de calcul de l'écart, à gain fixe.
    //Serial.println(ecart);
    //Bloc intégrateur : il faut que l'écart soit positif et la valeur de l'intégrateur inférieure à la borne maximale,
    //Integrator bloc : the difference must be positive and the value of the integrator must be under the maximum limit
    if (ecart >= 0 && integ+ecart < sat_haut){
      integ +=ecart;
    }
    //Autre solution : l'écart est négatif et la valeur de l'intégrateur est supérieure à la borne inférieure,
    //Integrator bloc : the difference must be negative and the value of the integrator must be over the minimum limit
    else if(ecart <=0 && integ+ecart > sat_bas){
      integ +=ecart;
    }
    //Serial.println(integ);
    //Enfin, on envoie la valeur finale de la consigne, donc la valeur de sortie de l'intégrateur, à la fonction de mise sous puissance.
    //Finally, we send the value to the motor controller
    //Activation du moteur
    //Motor activation
    motorController.Enable();
    set_power(integ);
    puissance_servo = integ;
  }
  else{
    set_power(0);
    digitalWrite(26,LOW);
    puissance_servo = 0;
    integ = 0;
    //On met aussi le moteur en roue libre
     motorController.Disable();
  }

 
  
  //Affichage de la puissance instantanée et de la charge de la batterie et de la vitesse, avec rafraichissement toutes les 0.5 secondes
  //update of the screen to show the instataneous current, battery charge and speed, refresh every 0.5 seconds 
  if (float(millis())/500.0 == float(millis()/500)){
    lcd.setCursor(9, 0);
    //lcd.print(get_power_filtre(courant_filtre));
    lcd.print(courant_filtre);
    //On ne mesure la tension de la batterie que si le moteur ne prélève pas de courant, ça risque de fausser la mesure sinon. On n'affiche que si la batterie n'est pas en sécurité.
    //The battery tension must be measured only if the motor isn't powered, otherwise the measurement will be false.
    if (puissance_servo == 0 && secu_batterie == false){
      lcd.setCursor(2, 1);
      lcd.print(get_capacity());
    }
    //Remise à zero de la vitesse si ça fait trop longtemps qu'aucun signal n'a été détecté.
    //The speed is put to 0 if no signal has been detected for too long.
    if (millis() - timer_speed > 3000){
      vitesse = 0;
      afficher_vitesse(vitesse);
    }
  }

  //Détermination de la vitesse filtrée:
  //Computing of the speed.
  //On détermine à chaque fois le front montant, et on réactualise la mesure de la vitesse.
  //We detect the rising front, and then the computation of the speed is updated.
  if (digitalRead(27) == LOW && magnet_visible == false){ //Si la couleur mesurée est noire et que le programme croit que la couleur est blanche
    magnet_visible = true;//Mise à jour du paramètre //update of the parameter
    //La roue tourne à priori, on met compteur à la date actuelle
    //update of the counter
    delta_t = (float(millis())-timer_speed);
    timer_speed = millis();
    Serial.println(delta_t);
    vitesse = 1000*(wheel_radius*2*3.14)/delta_t;
    vitesse = vitesse *3.6; //conversion en km/h
    afficher_vitesse(vitesse);
  }
  if (digitalRead(27) == HIGH && magnet_visible == true){ //Si la couleur mesurée est blanche et que le programme croit que la couleur est noire
    //If the measured color is white and the program thinks the color is black
    //On se contente sur cette mesure de détecter les fronts montants
    //We are satisfied with this measure to detect the rising fronts
    magnet_visible = false;//Mise à jour du paramètre
  }



  //Eclairage automatique NOT IMPLEMENTED
  //On met en place un comparateur à hysteresis pour éviter le clignotement
  if (analogRead(A3) < activate_light){
    is_light_activated = true;
  }
  if (analogRead(A3) > desactivate_light){
    is_light_activated = false;
  }
  if (is_light_activated = true){
    //Fermeture du relais
    digitalWrite(28,HIGH);
    //Affichage sur l'écran
    lcd.setCursor(12, 1);
    lcd.write((byte)2);
    lcd.setCursor(13, 1);
    lcd.write((byte)3);
  }
  else{
    //Ouverture du relais
    digitalWrite(28,LOW);
    //Affichage sur l'écran
    lcd.setCursor(12, 1);
    lcd.write("  ");
  }
}



//Fonction permettant de régler la puissance du moteur, entre 0 et 1.
//Function allowing to select the motor power, between 0 and 1.
void set_power(float consigne) {
  //On commence par implémenter la saturation : la consigne ne peut pas être inférieure à 0, et pas supérieure à 1.
  //Detection fo saturation : not over 1, not under 1.
  //Si c'est pas le cas, on la cast à la valeur la plus proche, 0 ou 1 ici.
  //If it's not the case, it's casted to the nearest value : 0 or 1.
  //Serial.println(consigne);
  if (consigne > 1){
    consigne = 1.0;
  }
  if (consigne < 0){
    consigne = 0.0;
  }
  //Ensuite, on met la consigne entre 0 et 255:
  //Then the value is put between 0 and 255
  float valeur_temp = consigne*255.0;
  //Enfin, on envoie la valeur au driver
  motorController.TurnLeft(valeur_temp);
  
  
  //Ancien code utilisé pour l'ancien controleur //Old piece of code, mifgt be useful with another controller, ask specifically about that if you're interested
  /*
  //2 cas : soit la consigne est supérieure à l'actuel, soit elle est inférieure. La consigne d'angle ne sera pas la même pour prendre en compte l'hysteresis. (Voir carnet p.81)
  //1er cas : consigne>puissance_servo
  //Mecanisme de protection : si la consigne est supérieure à 1 ou inférieure à 0 (ce qui ne devrait pas arriver), le système se met en butée
  if (consigne >= puissance_servo) {
    if (consigne <= 0) {
      servo_cont.write(40);
    }
    if (consigne >= 1) {
      servo_cont.write(165);
    }
    else {
      servo_cont.write(106.28 * consigne + 42.38);
    }
  }
  //2e cas : consigne<puissance_servo
  else {
    if (consigne <= 0) {
      servo_cont.write(40);
    }
    if (consigne >= 1) {
      servo_cont.write(160);
    }
    else {
      servo_cont.write(104.15 * consigne + 39.23);
    }
  }
  puissance_servo = consigne;
  */
}


//Fonction permettant de récupérer le courant alimentant le moteur
//Function measuring the current passing through the motor
float get_Current() {
  int temp  = analogRead(A1);
  float temp2 = ((((float(temp)) / 1024.0) * 5.0 )-U0)* float(gain_courant);
  //Serial.println(temp2);
  return (temp2);
}


//Fonction permettant d'écrire un message sur une certaine ligne et une certaine colonne sur l'écran lcd.
//Function allowing the program to write on a certain ligne and column on the lcd screen
void print_message(int ligne, int colonne, String message) {
  lcd.setCursor(ligne, colonne);
  lcd.print(message);
}

//Fonction permettant de récupérer le niveau de charge de la batterie, pour l'affichage
//Function allowing to measure the level of the battery, and cast it to be displayed on the screen.
String get_capacity() {
  //la batterie sera connectée au port A2 par un pont diviseur de tension de rapport 1/5
  //The battery will be connected to A2 through a 1/5 tension diviser bridge. 
  int tension = analogRead(A2);
  //On suppose la batterie chargée à 100% lorsque la batterie a une tension supérieure à 4.2V par élément(21V)
  //If the tension is over 21V, it's completely charged.
  if (tension >= 850){
    return("100");
  }
  //On suppose la batterie déchargée lorsque la batterie a une tension inférieure à 3.2V par élément(16V)
  //If the tension is under 16V, it's completely discharged
  if (tension <= 666){
    return ("00");
  }
  //Sinon, on suppose une décharge linéaire
  //Otherwise, a linear discharge is supposed.
  int ecart = tension-666;
  float pourcentage = float(ecart)/(850-666);
  pourcentage = pourcentage*100;
  ecart = int(pourcentage);
  return(String(ecart));
}

//Fonction permettant de retourner la puissance, sous forme de String de 3 lettres pour l'affichage sur l'écran
//Fonction allowing to get the powern and cast it to be displayed on the screen.
String get_power(){
  float puissance = puissance_servo*18*(float)get_Current();
  int temp = (int)puissance;
  if (puissance <0){
    return("000");
  }
  if (puissance < 10){
    return("00" + (String)temp);
  }
  else if (puissance < 100){
    return("0" + (String)temp);
  }
  return((String)temp);
}
String get_power_filtre(float courant_filtre){
  float puissance = puissance_servo*18*courant_filtre;
  //float puissance = courant_filtre*100;
  int temp = (int)puissance;
  if (puissance <0){
    return("000");
  }
  if (puissance < 10){
    return("00" + (String)temp);
  }
  else if (puissance < 100){
    return("0" + (String)temp);
  }
  return((String)temp);
}


//Fonction permettant de mettre à jour l'affichage de l'écran.
//Fonction allowing to update the informations on the screen. Not used
void update_screen(){
  //Affichage des écritures de base sur l'écran
  lcd.setCursor(0, 0); //colonne,ligne
  lcd.print("00km/h          "); //VITESSE A CODER!!
  lcd.setCursor(2, 1);
  lcd.print(get_capacity());
  lcd.print("%");
  lcd.setCursor(8, 0);
  lcd.print(get_power());
}

//Fonction permettant d'afficher la vitesse instantanée sur l'écran
//Fonction allowing to display the instantaneous speed on the screen.
void afficher_vitesse(float vitesse){
  lcd.setCursor(0, 0); //colonne,ligne
  int vitesse_int = int(vitesse);
  if (vitesse_int < 10){
    lcd.print("0");
    lcd.print(vitesse_int);
  }
  else{
    lcd.print(vitesse_int);
  }
}