// Gestion de la position
// du moteur logidule
//-------------------------------
// Version du 28/03/2018

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

// Poussoir et LEDs du LaunchPad
#define PousOn !(P1IN & (1<<3))
#define LedRougeOn P1OUT |= (1<<0)
#define LedRougeOff P1OUT &=~(1<<0)
#define LedVerteOn P1OUT |= (1<<6)
#define LedVerteOff P1OUT &=~(1<<6)

void InitPousLed () {
  P1DIR |=(1<<0)|(1<<6); // LEDs en sortie
  P1REN |=(1<<3); P1OUT |=(1<<3); // Poussoir en entrée avec Pull-up
}

// Définitions du moteur Logidule :
#define MoteurOut P1OUT // connecteur noté "série"
#define bMoteurEn 1
#define bMoteurDir 2

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

#define FinCouresIn P2IN // connecteur noté "P2-2-3"
#define bFinGauche 2
#define bFinDroite 3
#define FinCourseAvance !(FinCouresIn&(1<<bFinGauche))
#define FinCourseRecule !(FinCouresIn&(1<<bFinDroite))

#define EncodeursIn P2IN // connecteur noté "P2-0-1"
#define bEncodeurDroite 0
#define bEncodeurGauche 1
#define EncodeurDroite (EncodeursIn&(1<<bEncodeurDroite))>>bEncodeurDroite
#define EncodeurGauche (EncodeursIn&(1<<bEncodeurGauche))>>bEncodeurGauche

void InitMoteur () {
  P2DIR |= (1<<bMoteurEn)|(1<<bMoteurDir); // sorties EN et DIR
  P2REN |= (1<<bFinGauche)|(1<<bFinDroite); // pull-up Fin courses
  P2OUT |= (1<<bFinGauche)|(1<<bFinDroite); // pull-UP
  P2REN |= (1<<bEncodeurGauche); // pull-up Encodeur
  P2OUT |= (1<<bEncodeurGauche); // pull-UP
  P1REN |= (1<<bEncodeurDroite); // pull-up Encodeur
  P1OUT |= (1<<bEncodeurDroite); // pull-UP
}

// Gestion du PWM
uint8_t Intensite; // vitesse positive (8 bits)
uint8_t cptPwm; // compteur modulo 256
void InitPwm () {
  Intensite = 0;
  InitMoteur();
}

void GerePwm () {
  volatile uint16_t i;

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

int16_t Position;
uint8_t X, Y, AncienX, AncienY;

void InitPosition() {
  AncienX = EncodeurDroite; AncienY = EncodeurGauche;
  Position = 0;
}

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

// Programme principal :
int main() {
  WDTCTL = WDTPW+WDTHOLD; // stoppe le WatchDog
  BCSCTL1 = CALBC1_16MHZ; DCOCTL = CALDCO_16MHZ;
  P2SEL = 0;

  // Initialisations :
  InitPousLed();
  InitPwm();
  InitPosition();

  while (1) { // Boucle infinie :
    if (PousOn) { // seulement lorsqu'on presse le bouton
      Intensite = 128; // 50% de 256
      if (FinCourseAvance) {
        MoteurDirRecule;
      }
      if (FinCourseRecule) {
        MoteurDirAvance;
      }
    } else {
      Intensite = 0;
    }
    GerePwm();
    GerePosition();
    if (Position & (1<<1)) LedRougeOn; else LedRougeOff;
    if (Position & (1<<0)) LedVerteOn; else LedVerteOff;
  }
}