// Va et vient moteur Logidule (carte MSP-EXP430F5529)
// EPFL 2020, Pierre-Yves Rochat, pyr@pyr.ch
// 06.04.2023 : Version modifiée pour câblage compatible avec toutes les cartes blanches
// Corrigé TP gestion position et asservissement

#include <msp430.h>
#include <stdint.h>

#include <PYR_CarteBlanche.h>

// Définitions du moteur Logidule

#define MoteurDirDroite (P3OUT|=(1<<0))
#define MoteurDirGauche (P3OUT&=~(1<<0))
#define MoteurEn (P4OUT|=(1<<7))
#define MoteurStop (P4OUT&=~(1<<7))

#define EncodeurX (P2IN>>1)&1
#define EncodeurY (P2IN>>3)&1
#define FinCourseDroite (!(P3IN&(1<<2)))
#define FinCourseGauche (!(P3IN&(1<<1)))

void InitMoteur() {
  P4DIR |= (1<<7); // EN
  P3DIR |= (1<<0); // DIR

  P2DIR &=~(1<<1); P2REN |= (1<<1); P2OUT |= (1<<1); // encodeur X
  P2DIR &=~(1<<3); P2REN |= (1<<3); P2OUT |= (1<<3); // encodeur Y
  P3DIR &=~(1<<1); P3REN |= (1<<1); P3OUT |= (1<<1); // fin course gauche
  P3DIR &=~(1<<2); P3REN |= (1<<2); P3OUT |= (1<<2); // fin course droite
}


// Gestion du PWM :
uint8_t Intensite; // vitesse positive (8 bits)
uint8_t  cptPwm; // compteur module 256

void InitPwm () {
  Intensite = 0;
  InitMoteur();
}

void GerePwm () {
  volatile uint16_t i;

  if (cptPwm==0) {
    if (Intensite != 0) MoteurEn; else MoteurStop;
  }
  if (cptPwm==Intensite) {
    MoteurStop;
  }
  for (i=0; i<60; i++) {} // petite attente
  cptPwm++;
}

// Gestion de la position :
unsigned int Position;
unsigned char X, Y, AncienX, AncienY;

void InitPosition() {
  AncienX = EncodeurX; AncienY = EncodeurY;
  Position = 0;
}

void GerePosition() {
  X = EncodeurX; Y = EncodeurY;
  if ((X!=AncienX) || (Y!=AncienY)) { // 1 flanc sur X ou Y
    if (X == AncienY) Position++; else Position--;
    AfficheLedBleues(Position);
  }
  AncienX = X; AncienY = Y;
}

int16_t Commande, Cible, kP;

// Programme principal :
int main() {
  WDTCTL = WDTPW+WDTHOLD; // stoppe le WatchDog
  setupDCO(); // passe la fréquence du processeur à 25 MHz

  // Initialisations :
  InitCarteBlanche();
  InitPwm();
  InitPosition();
  Cible = 0;
  kP = 15;

  while (1) { // Boucle infinie :
    GerePwm();
    GerePosition();

    if (cptPwm==0) {

      // Asservissement :
      Commande = (Cible - Position) * kP;

      if (Commande>255) Commande=255;
      if (Commande<-255) Commande=-255;
      if (Commande<0) {
        MoteurDirDroite;
        Intensite = -Commande;
      } else {
        MoteurDirGauche;
        Intensite = Commande;
      }
    }

  }
}