// Va et vient moteur Logidule (carte MSP-EXP430F5529)
// EPFL 2020, Pierre-Yves Rochat, pyr@pyr.ch

#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&(1<<3))
#define FinCourseDroite (!(P3IN&(1<<2)))
#define FinCourseGauche (!(P3IN&(1<<1)))

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

  P2DIR &=~(1<<1); // encodeur X
  P2DIR &=~(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
}

void main(void) {
  WDTCTL = WDTPW | WDTHOLD;
  setupDCO(); // Fréquence à 25 MHz
  InitCarteBlanche();
  InitMoteur();

  while(1) { // Boucle infinie :
    // Active le moteur si un poussoir est activé :
    if (Pous1On || Pous2On) { MoteurEn; } else { MoteurStop; }

    if (EncodeurX) { Led1On; } else { Led1Off; } // affiche encodeur X
    if (EncodeurY) { Led3On; } else { Led3Off; } // affiche encodeur X

    if (FinCourseGauche) { // détection de fin de rotation à gauche
      MoteurDirDroite; // changement de sens
      Led4On; // affiche fin de course droite
    } else {
      Led4Off;
    }
    if (FinCourseDroite) { // idem pour fin de rotation à droite
      MoteurDirGauche;
      Led5On;
    } else {
      Led5Off;
    }
  }
}