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

// Va-et-vient du moteur Logidule
// Pierre-Yves Rochat, EPFL 2020, pyr@pyr.ch

// Poussoir et LED de la carte Launchpad MSP430F5529 (rouge) :
#define Pous1On (!(P2IN&(1<<1)))
#define Pous2On (!(P1IN&(1<<1)))
#define LedRougeOn P1OUT |= (1<<0)
#define LedRougeOff P1OUT &=~(1<<0)
#define LedRougeToggle P1OUT ^= (1<<0)
#define LedVerteOn P4OUT |= (1<<7)
#define LedVerteOff P4OUT &=~(1<<7)
#define LedVerteToggle P4OUT ^= (1<<7)

void InitCarteRouge () {
  P2REN |= (1<<1); P2OUT |= (1<<1); // poussoir1 , pull-up
  P1REN |= (1<<1); P1OUT |= (1<<1); // poussoir1 , pull-up
  P1DIR |= (1<<0); // LED rouge
  P4DIR |= (1<<7); // LED verte
}

// Définitions pour moteur Logidule sur la carte Launchpad MSP430F5529 (rouge) :
#define MoteurOut P4OUT
#define bMoteurEn 2
#define bMoteurDir 1

#define MoteurTourne MoteurOut|=(1<<bMoteurEn)
#define MoteurStop MoteurOut&=~(1<<bMoteurEn)
#define MoteurDirAvance MoteurOut&=~(1<<bMoteurDir)
#define MoteurDirRecule MoteurOut|=(1<<bMoteurDir)

#define FinDroiteIn P3IN
#define bFinDroite 0
#define FinGaucheIn P2IN
#define bFinGauche 0
#define FinCourseAvance !(FinGaucheIn&(1<<bFinGauche))
#define FinCourseRecule !(FinDroiteIn&(1<<bFinDroite))

#define EncodeurXIn P2IN
#define bEncodeurX 7
#define EncodeurYIn P8IN
#define bEncodeurY 1
#define EncodeurX (EncodeurXIn&(1<<bEncodeurX))>>bEncodeurX // valeurs 0 ou 1
#define EncodeurY (EncodeurYIn&(1<<bEncodeurY))>>bEncodeurY

void InitMoteur () {
  // Les encodeurs et les fins de course n'ont pas besoin de pull-up
  P4DIR |= (1<<bMoteurEn); // EN en sortie
  P4DIR |= (1<<bMoteurDir); // DIR en sortie
}

// Programme Principal :

int main() {
  WDTCTL = WDTPW + WDTHOLD;
  InitCarteRouge();
  InitMoteur(); MoteurStop; MoteurDirAvance;

  while(1) {
    if (Pous1On || Pous2On) { // rotation lorsqu'on presse un bouton-poussoir
      MoteurTourne;
      if (FinCourseAvance) {
        MoteurDirRecule;
      }
      if (FinCourseRecule) {
        MoteurDirAvance;
      }
    } else {
      MoteurStop;
    }
    // Affiche les encodeurs sur les LED :
    if (EncodeurX) { LedRougeOn; } else { LedRougeOff; }
    if (EncodeurY) { LedVerteOn; } else { LedVerteOff; }
  }
}