#include "msp430x14x.h"

#define PWM_Period      3999                    // 8MHz / 4000 = 2kHz
#define PWM_Max         1199                    // Max duty factor used
#define PWM_Min         149                     // Min duty factor used

#define MIN_SPEED       10000                   // Min. measured speed

int Array[700];                                 // Debug buffer
int pArray = 0;

unsigned int SetSpeed = 1667;                   // 1667 equal 50rpsec
                                                // SetSpeed = f_Timer / (RevPerSec * 24)
                                                // with f_Timer = 2MHz

unsigned int LastTACCR;                         // Speed measurement vars
unsigned int CurrentSpeed = MIN_SPEED;
unsigned long SpeedMemSum = 8 * (unsigned long)MIN_SPEED;
unsigned int pSpeedMem = 0;
unsigned int SpeedMem[8] =
{
  MIN_SPEED, MIN_SPEED, MIN_SPEED, MIN_SPEED,
  MIN_SPEED, MIN_SPEED, MIN_SPEED, MIN_SPEED
};

long Error;                                     // Control algorithm vars
long dError;
long LastError;
long PWMValue;

int X1[5];                                      // Fuzzy control variables
int X2[5];
int Y[5];
int Output;

#define NM          0                           // negative medium
#define NS          1                           // negative small
#define ZE          2                           // zero equal
#define PS          3                           // positive small
#define PM          4                           // positive medium

const unsigned char InferenceTable[5][5] =
{
  { PM, PM, PM, PS, ZE },
  { PM, PM, PS, ZE, NS },
  { PM, PS, ZE, NS, NM },
  { PS, ZE, NS, NM, NM },
  { ZE, NS, NM, NM, NM }
};

const signed char OutputFunc[5] =
{
  -0x10, -0x08, 0, 0x08, 0x10
};

// Function prototypes
void InitSystem(void);
void Fuzzification(int Value, int *Data);
void FuzzyInference(int *Error, int *dError, int *Y);
int Defuzzification(int *Y);
__interrupt void WDT_ISR(void);
__interrupt void TimerA0_ISR(void);
__interrupt void TimerA1_ISR(void);

void main(void)
{
  volatile unsigned int i;

  InitSystem();

  while (1)
  {
    __bis_SR_register(LPM0_bits);               // Enter LPM, wait for start of
                                                // next control cycle

    // If Error > 0 then motor is to fast
    // If Error < 0 then motor is to slow

    LastError = Error;

    __disable_interrupt();                      // Protect following statements
    Error = (long)SetSpeed - CurrentSpeed;      // Calc absolute error

    if (pArray < sizeof Array/sizeof Array[0])  // Store some values for debug
      Array[pArray++] = CurrentSpeed;

    __enable_interrupt();

    Error <<= 3;                                // Multiply by 8

    dError = Error - LastError;                 // Calc differential error
    dError <<= 5;                               // Multiply by 32

    // Ensure error is within Fuzzy boundaries
    if (Error > 0xc00)                          // Motor too fast?
    {
      TBCCR2 = PWM_Min;                         // Set PWM to minimum
      continue;                                 // Skip over control algorithm
    }

    if (Error < -0xc00)                         // Motor too slow?
    {
      TBCCR2 = PWM_Max;                         // Set PWM to maximum
      continue;                                 // Skip over control algorithm
    }

    Fuzzification(Error, X1);                   // Transform absolute error
    Fuzzification(dError, X2);                  // Transform differential error
    FuzzyInference(X1, X2, Y);                  // Apply Fuzzy rule table
    Output = Defuzzification(Y);                // Obtain scalar result

    PWMValue = TBCCR2 + Output;
    if (PWMValue < PWM_Min) PWMValue = PWM_Min; // Limit output value
    if (PWMValue > PWM_Max) PWMValue = PWM_Max;
    TBCCR2 = PWMValue;                          // Assign new PWM duty cycle
  }
}
//------------------------------------------------------------------------------
// Function converts the discrete input value 'Value' into the 5-element
// Fuzzy vector Data[].
//------------------------------------------------------------------------------
void Fuzzification(int Value, int *Data)
{
  int i;

  for (i = 0; i < 5; i++)
    Data[i] = 0;

  if (Value < -0x800)
    Data[NM] = 0x400;
  else if (Value < -0x400)
  {
    Data[NM] = 0x400 - (Value + 0x800);
    Data[NS] = Value + 0x800;
  }
  else if (Value < 0)
  {
    Data[NS] = 0x400 - (Value + 0x400);
    Data[ZE] = Value + 0x400;
  }
  else if (Value < 0x400)
  {
    Data[ZE] = 0x400 - Value;
    Data[PS] = Value;
  }
  else if (Value < 0x800)
  {
    Data[PS] = 0x400 - (Value - 0x400);
    Data[PM] = Value - 0x400;
  }
  else
    Data[PM] = 0x400;
}
//------------------------------------------------------------------------------
// Function applies the Fuzzy control interference rule table InferenceTable[][]
// to the two input arrays X1[] and X2[] to generate the output vector Y[].
//------------------------------------------------------------------------------
void FuzzyInference(int *X1, int *X2, int *Y)
{
  int min[5];
  int max;
  int maxpos;
  int i, j;

  for (i = 0; i < 5; i++)                       // Clear output vector Y[]
    Y[i] = 0;

  for (i = 0; i < 5; i++)                       // Loop through X1[]
  {
    for (j = 0; j < 5; j++)                     // Loop through X2[]
      if (X1[i] < X2[j])                        // Determine smaller value,
        min[j] = X1[i];                         // store into min[]
      else
        min[j] = X2[j];

    max = min[0];                               // Find maximum in min[]
    maxpos = 0;
    for (j = 1; j < 5; j++)
      if (max < min[j])
      {
        max = min[j];                           // Store maximum
        maxpos = j;                             // Store position of maximum
      }

    if (max > Y[InferenceTable[i][maxpos]])     // Apply inference table
      Y[InferenceTable[i][maxpos]] += max;

    if (Y[InferenceTable[i][maxpos]] > 0x400)   // Limit output vector elements
      Y[InferenceTable[i][maxpos]] = 0x400;
  }
}