/************* BROCHES *************/
#define ENCA 2
#define ENCB 3
#define IN1  8
#define IN2  9
#define ENA  10

/************* PARAMÈTRES MOTEUR *************/
#define RATIO 9.6
#define CPR_MOTOR 11
#define CPR_TOTAL (CPR_MOTOR * RATIO)

/************* FILTRE *************/
#define ALPHA 0.7

/************* CORRECTEUR PI *************/
#define KP 2
#define KI 3

/************* VARIABLES *************/
volatile long posi = 0;
long lastPosi = 0;

unsigned long t0, prevT;
float t, deltaT;

float rpm = 0;
float rpmFilt = 0;
float rpmRef = 0;

float error = 0;
float integral = 0;
float control = 0;

/**************** SETUP ****************/
void setup() {

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);

  pinMode(ENCA, INPUT);
  pinMode(ENCB, INPUT);

  attachInterrupt(digitalPinToInterrupt(ENCA), readEncoderA, RISING);

  Serial.begin(9600);
  Serial.println("t,rpm_ref,rpm,rpmFilt");

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  t0 = micros();
  prevT = t0;
}

/**************** LOOP ****************/
void loop() {

  /************* ÉCHELON 4s / 4s *************/
  unsigned long t_ms = millis() % 8000;

  if (t_ms < 4000) {
    rpmRef = 300;   // vitesse de référence
  } else {
    rpmRef = 0;
    integral = 0;   // reset intégrale à l'arrêt
  }

  /************* TEMPS *************/
  unsigned long currT = micros();
  deltaT = (currT - prevT) / 1.0e6;
  prevT = currT;

  if (deltaT <= 0) return;

  t = (currT - t0) / 1.0e6;

  /************* POSITION *************/
  long pos;
  noInterrupts();
  pos = posi;
  interrupts();

  long deltaP = pos - lastPosi;
  lastPosi = pos;

  /************* VITESSE *************/
  rpm = (deltaP / deltaT) * (60.0 / CPR_TOTAL);
  rpmFilt = ALPHA * rpmFilt + (1.0 - ALPHA) * rpm;

  /************* CORRECTEUR PI *************/
  error = rpmRef - rpmFilt;
  integral += error * deltaT;

  control = KP * error + KI * integral;

  control = constrain(control, 0, 255);
  analogWrite(ENA, control);

  /************* AFFICHAGE *************/
  Serial.print(t, 3);
  Serial.print(",");
  Serial.print(rpmRef, 1);
  Serial.print(",");
  Serial.print(rpm, 1);
  Serial.print(",");
  Serial.println(rpmFilt, 1);

  delay(10);
}

/**************** INTERRUPTION ****************/
void readEncoderA() {
  if (digitalRead(ENCB)) posi++;
  else posi--;
}