1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
| #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;
}
}
//------------------------------------------------------------------------------
// 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.
//------------------------------------------------------------------------------
WDT_ISR(Wdt_ISR)
__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.
//------------------------------------------------------------------------------
TIMERA0_ISR(TimerA0_ISR)
__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.
//------------------------------------------------------------------------------
TIMERA1_ISR(TimerA1_ISR)
__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
}
} |