// Moteur Logidule et carte MSP-EXP430F5529)
// Réglage P, avec un consigne variable (dent de scie)
// (avec PWM et lecture de la position et affichage sur LCD)
// EPFL 2021, Pierre-Yves Rochat, pyr@pyr.ch

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

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

volatile int16_t pwmMoteur; // commande du moteur, valeur signée +-15 bit
volatile int16_t position; // position angulaire de l'axe
volatile uint8_t debCycle; // Début du cycle du PWM, pour synchronisation
volatile uint8_t X, ancienX, Y, ancienY;

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

  // Timer 1 utilisé pour le PWM du moteur :
  TA1CTL = TASSEL_2 | ID_0 | MC_2 | TAIE;
  // Interrupt sur CCR1
  TA1CCTL1 = CCIE;

  // Interruption sur X et Y pour gérer la position :
  P2IES = P2IN; // prépare le sens des prochains fronts
  P2IE |= (1<<1);// |(1<<3);

  __enable_interrupt(); // Active l'ensemble des interruption

  position = 0;
  ancienX = EncodeurX;
  ancienY = EncodeurY;
}

void GereEncodeur() {
  X = EncodeurX; Y = EncodeurY; // valeurs actuelles
  if (ancienX!=Y) {  // détection du sens
    position--;
  } else {
    position++;
  }
  // Affiche les 3 bits de poids faible sur Led1 à Led3 :
  if (position & (1<<0)) { Led1On; } else { Led1Off; }
  if (position & (1<<1)) { Led2On; } else { Led2Off; }
  if (position & (1<<2)) { Led3On; } else { Led3Off; }
  // Affiche les 5 bits suivante sur Led4 à Led8 :
  AfficheLedBleues(position>>3);
  ancienX = X; ancienY = Y; // anciennes valeurs mémorisées
}


#pragma vector=PORT2_VECTOR // Interruption Port2
__interrupt void Port_2(void) {
  if (P2IFG & (1<<1)) {
    P2IES ^= (1<<1); // inverse le front
    P2IFG &=~(1<<1); // quittance l'événement
  }
  if (P2IFG & (1<<3)) {
    P2IES ^= (1<<3);
    P2IFG &=~(1<<3);
  }
  GereEncodeur(); // met à jour la position en fonction de X et Y
}

#pragma vector=TIMER1_A1_VECTOR // Interruption A1 du Timer 1
__interrupt void Timer1_A1(void) {
  switch(TA1IV) {
  case  2: // CCR1 : fin du cycle
      MoteurStop;
    break;
  case  4: break; // CCR2
  case  6: break; // CCR3
  case  8: break; // CCR4
  case 14: // Overflow : début du cycle
    // La commande va de -32'768 à +32'767 (recule et avance)
    // Il faut donc multiplier sa valeur absolue pour que la valeur
    // du registre de comparaison prenne des valeurs de 0 à 65'535
    if (pwmMoteur>0) { TA1CCR1 = pwmMoteur*2; MoteurDirDroite; MoteurEn; }
    if (pwmMoteur<0) { TA1CCR1 = -pwmMoteur*2; MoteurDirGauche; MoteurEn; }
    debCycle = 1;
    break;
  }
}

#define KP1 (int32_t)5000 // facteur proportionnel
#define KP2 (int32_t)1000

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

  Dogs102x6_init(); Dogs102x6_backlightInit(); // Init LCD
  Dogs102x6_setBacklight(5); Dogs102x6_setContrast(11);

  InitCarteBlanche();
  InitMoteur();

  uint16_t temps = 0;
  int16_t x = 102; // force l'effacement de l'écran au début
  int32_t consigne, erreur, commande; // calcul sur 32 bits
  int32_t kp = KP1;

  while(1) { // Boucle infinie :
    while(debCycle==0) { } // synchronise avec le cycle du PWM
    debCycle = 0;

    if (Pous2On) {kp = KP2; } else { kp = KP1; } // choix du facteur k

    if (x<5) {
        consigne = 0;
    } else if (x<55) {
        consigne = (x-5) >> 1;
    } else if (x<70) {
            consigne = -15;
    } else {
        consigne = 0;
    }

    erreur = position - consigne;
    commande = -kp * erreur; // réglage proportionnel

    if (commande>(int32_t)32767) { pwmMoteur = 32767; }
    else if (commande<(int32_t)-32768) { pwmMoteur = -32768; }
    else  { pwmMoteur = (int16_t)commande; }

    temps++;
    if (Pous1On) { // relance un cycle
      temps = 0; x = 0;
      Dogs102x6_clearScreen();
      Dogs102x6_verticalLineDraw(0, 63, 0, 0);
      Dogs102x6_horizontalLineDraw(0, 101, 32, 0);
    }
    if ((temps & 0x1F)==0) {
      if (x < 102) {
        Dogs102x6_pixelDraw(x, 32-(position), 0);
        Dogs102x6_pixelDraw(x, 32-(consigne), 0);
        Dogs102x6_pixelDraw(x, 63, 1);
        x++;
        Dogs102x6_pixelDraw(x, 63, 0);
      }
    }
  }
}