fuzzy_control_iar.c

模糊控制程序

//------------------------------------------------------------------------------
// MSP430F14x Fuzzy Logic Motor Control Demo
//
// Description: Program controls the speed of a universal serial motor using
// a Fuzzy logic control algorithm. The actual motor speed is determined by
// measuring time between transitions from the optical encoder. The target
// speed is programmed into the variable SetSpeed.
//
//             MSP430F14x
//          +---------------+
//          |               |
//          |           P2.2|<--- Optical encoder, 24 pulses per rev
//          |               |
//          |           P4.2|---> PWM output drive stage
//          |               |
//          |       XIN/XOUT|<--- 8MHz crystal
//          |               |
//          +---------------+
//
// Andreas Dannenberg
// Texas Instruments Inc.
// January 2005
// Built with IAR Embedded Workbench Version: 3.21A
//
// Courtesy of Dr. Odry P閠er, Div閗i Szabolcs, Csasznyi Andor, B鷕醤y N醤dor
//------------------------------------------------------------------------------
// THIS PROGRAM IS PROVIDED "AS IS". TI MAKES NO WARRANTIES OR
// REPRESENTATIONS, EITHER EXPRESS, IMPLIED OR STATUTORY,
// INCLUDING ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS
// FOR A PARTICULAR PURPOSE, LACK OF VIRUSES, ACCURACY OR
// COMPLETENESS OF RESPONSES, RESULTS AND LACK OF NEGLIGENCE.
// TI DISCLAIMS ANY WARRANTY OF TITLE, QUIET ENJOYMENT, QUIET
// POSSESSION, AND NON-INFRINGEMENT OF ANY THIRD PARTY
// INTELLECTUAL PROPERTY RIGHTS WITH REGARD TO THE PROGRAM OR
// YOUR USE OF THE PROGRAM.
//
// IN NO EVENT SHALL TI BE LIABLE FOR ANY SPECIAL, INCIDENTAL,
// CONSEQUENTIAL OR INDIRECT DAMAGES, HOWEVER CAUSED, ON ANY
// THEORY OF LIABILITY AND WHETHER OR NOT TI HAS BEEN ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGES, ARISING IN ANY WAY OUT
// OF THIS AGREEMENT, THE PROGRAM, OR YOUR USE OF THE PROGRAM.
// EXCLUDED DAMAGES INCLUDE, BUT ARE NOT LIMITED TO, COST OF
// REMOVAL OR REINSTALLATION, COMPUTER TIME, LABOR COSTS, LOSS
// OF GOODWILL, LOSS OF PROFITS, LOSS OF SAVINGS, OR LOSS OF
// USE OR INTERRUPTION OF BUSINESS. IN NO EVENT WILL TI'S
// AGGREGATE LIABILITY UNDER THIS AGREEMENT OR ARISING OUT OF
// YOUR USE OF THE PROGRAM EXCEED FIVE HUNDRED DOLLARS
// (U.S.$500).
//
// Unless otherwise stated, the Program written and copyrighted
// by Texas Instruments is distributed as "freeware".  You may,
// only under TI's copyright in the Program, use and modify the
// Program without any charge or restriction.  You may
// distribute to third parties, provided that you transfer a
// copy of this license to the third party and the third party
// agrees to these terms by its first use of the Program. You
// must reproduce the copyright notice and any other legend of
// ownership on each copy or partial copy, of the Program.
//
// You acknowledge and agree that the Program contains
// copyrighted material, trade secrets and other TI proprietary
// information and is protected by copyright laws,
// international copyright treaties, and trade secret laws, as
// well as other intellectual property laws.  To protect TI's
// rights in the Program, you agree not to decompile, reverse
// engineer, disassemble or otherwise translate any object code
// versions of the Program to a human-readable form.  You agree
// that in no event will you alter, remove or destroy any
// copyright notice included in the Program.  TI reserves all
// rights not specifically granted under this license. Except
// as specifically provided herein, nothing in this agreement
// shall be construed as conferring by implication, estoppel,
// or otherwise, upon you, any license or other right under any
// TI patents, copyrights or trade secrets.
//
// You may not use the Program in non-TI devices.
//------------------------------------------------------------------------------

#include "msp430x14x.h"

#define PWM_Period      3999                    // 8MHz / 4000 = 2kHz
#define PWM_Max         1199                    // Max duty factor used,最大脉宽150us
#define PWM_Min         149                     // Min duty factor used,最小脉宽18.625us

#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;
  }
}
//------------------------------------------------------------------------------
// Function transforms the Fuzzy vector Y[] to a discrete value using the
// center of gravity method.
//------------------------------------------------------------------------------
int Defuzzification(int *Y)
{
  int i;
  int ReturnVal = 0;
  int SumY = 0;

  for (i = 0; i < 5; i++)
  {
    SumY += Y[i];
    ReturnVal += Y[i] * OutputFunc[i];
  }

  return ((long)ReturnVal << 2) / SumY;         // Scale result by 4
}
//------------------------------------------------------------------------------
// MSP430-specific initialization of clock system and timer modules
//------------------------------------------------------------------------------
void InitSystem(void)
{
  volatile unsigned int i;

  WDTCTL = WDTPW + WDTHOLD;                     // Hold WDT

  // Setup Clock System
  BCSCTL1 |= XTS;                               // ACLK=LFXT1=HF XTAL

  do {
    IFG1 &= ~OFIFG;                             // Clear OSCFault flag
    for (i = 0xFF; i > 0; i--);                 // Time for flag to set
  } while (IFG1 & OFIFG);                       // OSCFault flag still set?

  BCSCTL2 |= SELM_3;                            // MCLK=LFXT1 (safe)

  // Setup Ports
  P2SEL = 0x04;                                 // Assign P2.2 to Timer_A.CCI0B
  P4SEL = 0x04;                                 // Assign P4.2 to Timer_B.OUT2
  P4DIR = 0x04;                                 // P4.2 output

  // Setup Timer_A for speed measurement
  TACCR1 = MIN_SPEED;                           // Set minimum speed that is read out
  TACCTL0 = CM_1 + CCIS_1 + SCS + CAP + CCIE;   // Capture rising edge of CCI0B, interrupt
  TACCTL1 = CCIE;                               // Compare mode, interrupt on EQU1
  TACTL = TASSEL_1 + ID_2 + MC_2;               // Use ACLK/4=2MHz, start in continuos mode

  // Setup Timer_B for PWM generation
  TBCCR0 = PWM_Period;                          // PWM Period
  TBCCTL2 = OUTMOD_7 + CLLD0;                   // Set OUT2 on EQU0, reset on EQU1,
                                                // sync latch load with EQU0
  TBCTL = TBSSEL_1 + MC_1;                      // Use ACLK, start in up-mode

  // Setup WDT for periodic interrupt
  WDTCTL = WDTPW + WDTTMSEL + WDTSSEL;          // Intervall timer, 8MHz/32768=244Hz
  IE1 |= WDTIE;

  __enable_interrupt();
}
//------------------------------------------------------------------------------
// The Watchdog-timer ISR is used to periodically wake-up from low-power mode
// LPM0 to execute the Fuzzy control loop.
//------------------------------------------------------------------------------
#pragma vector = WDT_VECTOR
__interrupt void WDT_ISR(void)
{
  __bic_SR_register_on_exit(LPM0_bits);         // Wake up from LPM
}
//------------------------------------------------------------------------------
// The Timer_A CCR0 ISR is called on each TACCR0 capture event to obtain
// the time stamp of the input signal transition. An 8-tap moving average
// filter is used to minimize measurement error.
//------------------------------------------------------------------------------
#pragma vector = TIMERA0_VECTOR
__interrupt void TimerA0_ISR(void)
{
  SpeedMemSum -= SpeedMem[pSpeedMem];           // Remove oldest value
  SpeedMem[pSpeedMem] = (unsigned int)(TACCR0 - LastTACCR);  // Replace with current
  SpeedMemSum += SpeedMem[pSpeedMem++];         // Update running sum
  CurrentSpeed = SpeedMemSum >> 3;              // Calc speed by div 8
  pSpeedMem &= 0x07;                            // Adjust circular pointer

  LastTACCR = TACCR0;
  TACCR1 = LastTACCR + MIN_SPEED;               // Set timeout for minimum speed
}                                               // to be read out
//------------------------------------------------------------------------------
// The Timer_A CCR1 ISR is called on TACCR1 compare events, which are generated
// when no input signal change is detected within 'MIN_SPEED' clock ticks after
// the last TACCR0 event. This provides a timeout function to update the speed
// variables in the case the motor gets halted.
//------------------------------------------------------------------------------
#pragma vector = TIMERA1_VECTOR
__interrupt void TimerA1_ISR(void)
{
  switch (TAIV)
  {
    case 0x02 :                                 // TACCR1 CCIFG
      SpeedMemSum -= SpeedMem[pSpeedMem];       // Remove oldest value
      SpeedMem[pSpeedMem] = (unsigned int)(TACCR1 - LastTACCR);  // Replace with current
      SpeedMemSum += SpeedMem[pSpeedMem++];     // Update running sum
      CurrentSpeed = SpeedMemSum >> 3;          // Calc speed by div 8
      pSpeedMem &= 0x07;                        // Adjust circular pointer

      LastTACCR = TACCR1;
      TACCR1 = LastTACCR + MIN_SPEED;           // Set timeout for minimum speed
      break;                                    // to be read out
  }
}