current_control.c 7.6 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
/*********************************************

 Current regulation - Aleks Ponjavic 18/09/07
           for LTH - reglerteknik

   Note that the voltage is actually a duty
   cycle for the PWM. The PWM is running at
  12V with a duty cycle set by a 10bit value

*********************************************/

//#include <math.h>		//Only include if sin will be used, takes up memory!
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>

18
19

// reference generation variables
20
volatile int16_t ref = 200;
21
22
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
23
24
25
26


// control variables
volatile unsigned char lbyte,hbyte;  
27
volatile int16_t y;
28
volatile uint8_t alt = 1;
29
30
volatile uint8_t low, high;

31
32
33
//test variables
volatile int16_t e = 0;
volatile int16_t v = 0;
34
volatile int16_t I = 0;
35
volatile int16_t u = 0;
36
37
38
volatile int16_t K = 74;//5;//375; // 7 frac bits
volatile int16_t Ke = 26;//2;//6; //7 frac bits, K*h/Ti
volatile int16_t Ksat = 44;//1; // 7 frac bits, h/Tr
39
40
41
42
43
44
45


// logging variables
#define log_len 100
volatile int16_t ctrl_log[log_len];
volatile int16_t error_log[log_len];
volatile int32_t I_log[log_len];
46
volatile int16_t skipSamples = 1000;
47
48
49
50
volatile int16_t countSamples = 0;
volatile int16_t jj=0;
volatile int8_t stop = 0;
volatile int16_t temp;
51

52

53
54
55
56
// pwm sync variables
volatile int8_t pwmCycles = 0;


57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
/* Routine used to set the red LED */
void setLED(uint8_t on)
{
  if (on) PORTB &= 0x7f;	//Turn on
  else PORTB |= 0x80;	//Turn off
}


/* Routine used to transmit bytes on the serial port */
static void putchar(unsigned char ch)
{
	while ((inp(UCSRA) & 0x20) == 0) {};
	outp(ch, UDR);
	while ((inp(UCSRA) & 0x20) == 0) {};
}

/* Interrupt service routine for handling incoming bytes on the serial port 
 might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}
76
77


78
79
80
81
82
83
84
85
static inline int16_t readInput() {
  uint8_t low, high;
  ADCSRA |= 0x40;
  while (ADCSRA & 0x40);
  low = ADCL;
  high = ADCH;
  return ((high<<8) | low) - 512;
}
86
	
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

/* Write 8-bit output using the PWM-generator */
static inline void writeOutput(int16_t val) {
  if (val < 0) {
    PORTC = 0x80+(PORTC & 0x7F);
    OCR1BH = 0;    //(unsigned char) (-val)&0xff00;
    OCR1BL = (unsigned char) (-val)&0x00ff;
  } else {
    PORTC = (PORTC & 0x7F);
    OCR1BH = 0;    //(unsigned char) (val&0xff00);
    OCR1BL = (unsigned char) (val&0x00ff);
  }
}


102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
/* Send logged data over Serial connection */
static inline void sendData() {
  int16_t ii = 0;
  while (ii < log_len) {

    putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8));
    putchar((unsigned char) (ctrl_log[ii]&0x00ff));

    putchar((unsigned char) ((error_log[ii]&0xff00)>>8));
    putchar((unsigned char) (error_log[ii]&0x00ff));

    putchar((unsigned char) ((I_log[ii]&0xff000000)>>24));
    putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16));

    putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
    putchar((unsigned char) (I_log[ii]&0x000000ff));


    ii++;
  }
}

124

125
126
127
128
129
130
131
132
133
/* Timer 1 compare match, start AD-conversion
 * Additional changes: Dont use free running AD-conversion
 *                     Enable overflow interrupts timer1
 */
SIGNAL(SIG_OUTPUT_COMPARE2) {
  // Start AD conversion
  ADCSRA |= BV(ADSC);
  
  // Code previously in sig_adc
134
  PORTA |= 0x10;	//Turn on calculation timer
135
    
136
137
138
  // Read input
  low = inp(ADCL);
  high = inp(ADCH);
139
  y =  ((high<<8) | low) - 512; //y 9 frac bits
140
  
141
142
  //control, since negative measurements
  e = ref+y; //e 9 frac bits
143
  
144
  
145
146
147
148
149
150
151
152
153
154
  //v = (int16_t)((K*e+I+64)>>7);
  v = (int16_t)(((K*e+64)>>7)+(temp>>4));
  //saturation and update integral part of ctrl
  if (v > 511) {
    temp = temp + (((Ke*e) + (Ksat)*(511-v))>>3);
  } else if (v < -512) {
    temp = temp + (((Ke*e) + (Ksat)*(-512-v))>>3);
  } else {
    temp = temp + ((Ke*e)>>3);
  }
155
  
156
157
158
  /*
  //saturation and update integral part of ctrl
  if (v > 511) {
159
  I = I +(int32_t)((Ke*e) + (Ksat)*(511-v));
160
  } else if (v < -512) {
161
  I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
162
  } else {
163
  I = I +(int32_t)(Ke*e);
164
165
  }
  */
166
  // write output, inverting mode means set pwm to 127-ctrl_out
167
168
  
  
169
  
170
171
  // Original styrning med 7 bitar + direction
  u = (v+2)>>2; //7 frac bits to pwm 
172
  
173
174
175
176
177
  if (u > 127) {
    u = 127;
  } else if (u < -128) {
    u = -128;
  }
178
  
179
  if (u < 0) {
180
    PORTC |= 0x80;
181
    OCR1BL = (unsigned char) (128-(-u));
182
183
  } else {
    PORTC = (PORTC & 0x7F);
184
      OCR1BL = (unsigned char) (127-u);
185
  }
186
187
  
  
188
189
190
191
  // For logging
  countSamples++;
  if (countSamples == skipSamples) {
    ctrl_log[jj] = y;
192
    I_log[jj] = u;
193
194
195
    error_log[jj] = e;
    jj++;
    countSamples = 0;
196
    
197
198
    // Stop after a while
    if ((jj == (log_len-1)) & !stop) {
199
200
201
	outp(0x7f,OCR1BL);
	stop = 1;
	sendData();
202
    }
203
  }
204
  
205
  PORTA &= 0xef;	//Turn off calculation timer  
206

207
208
209
}


210

211
212
/* reference square- or triangle wave generator with timer 0 */
SIGNAL(SIG_OVERFLOW0) {
213
  int8_t rectangle = 0;
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
  refCount++;
  if (rectangle == 1) {
    if (refFlag == 0) {
      if (refCount == 10) {
	refFlag = 1;
	ref = -ref;
	refCount = 0;
      }
    } else {
      if (refCount == 20) {
	ref = -ref;
	refCount = 0;
      }
    }
  } else {
229
230
    if (refCount <= 40) {  // ref
      ref -= 10;
231
    } else {
232
      ref += 10;
233
    }
234
    if (refCount == 80) { // ref*2
235
236
237
238
239
240
241
242
      refCount = 0;
    }
  }
}


int main()
{
243
  cli();
244
245
246
247
248
249
250
251
252
253
254
255
256
  int i,j;
  
  //Port directions
  outp(0x80,PORTB); // LED off
  outp(0x80,DDRB);  // output on LED
  outp(0x08,PORTC); // pull up on overtemp signals
  outp(0xa0,DDRC);  // output on dir and brake
  outp(0x80,PORTD); // pull up on reset switch
  outp(0x10,DDRD);  // output on pwm for motor 1
  
  outp(0x10,DDRA);  // test pin output

  /* Timer section */
257
  // Enable timer0 overflow interrupts
258
259
  // and enable timer1 overflow interrupts for pwm-control sync
  outp(BV(TOIE0)|BV(OCIE2),TIMSK);
260
  
261
262
263
  // Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high))
  outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|BV(WGM11)|BV(WGM10),TCCR1A);
  outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
264
  
265
  // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> h_pwm=8.61 micros)
266
  // 7 bit ctrl-signal and direction
267
  outp(0x00,OCR1AH);
268
269
  outp(0x7f,OCR1AL);
  
270
271
  outp(0x00,TCNT1H);
  outp(0x00,TCNT1L);
272
  
273
  // 7 bit ctrl-signal and direction
274
  outp(0x00,OCR1BH);
275
  outp(0x7f,OCR1BL); // to not start motor-rotation before control
276
277

  
278
279
280
  /* Timer 2, 4 times pwm-period, for control sampling, prescaler 8 */
  outp(BV(WGM21)|BV(CS21),TCCR2);
  outp(0x3f,OCR2);
281
  /* Reset timer 2 */
282
  outp(0,TCNT2);
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297

  /* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */
  outp(BV(CS02)|BV(CS00),TCCR0);
  
  //Serial communication
  outp(0x00, UCSRA);	// USART:
  outp(0x98, UCSRB);	// USART: RxIntEnable|RxEnable|TxEnable
  outp(0x86, UCSRC);	// USART: 8bit, no parity
  outp(0x00, UBRRH);	// USART: 115200 @ 14.7456MHz
  outp(7,UBRRL);	// USART: 115200 @ 14.7456MHz
  

  /* AREF (AREF is 5V) pin external capacitor, MUX0 for current, MUX3?? for pendulum angle */
  outp(BV(REFS0)|BV(MUX0),ADMUX); 	
  
298
299
300
301
302
303
304
305
306
  // Enable ADC interrupts, start first conversion, prescaler 32, free running mode
  //outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);

  // Enable ADC interrupts, start first conversion, prescaler 32, not free running mode
  //outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);

  // Enable ADC, start first conversion, prescaler 32, not free running mode
  outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);

307
308
309
310
311

  // outp(BV(ADEN)|BV(ADSC),ADCSRA);
  
  /* Wait a little bit, probably not needed...*/
  int tmp;
312
313
314
  //  for(i=0;i<2000;i++) 
  //   for(j=0;j<400;j++)
  //   tmp = j*j*j;
315
316
317
318
319
320
  

  //Enable interrupts
  sei();

  // loop
321
322
323
  while (1) {
    if (stop) {
      cli();
324
      OCR1BL = 0xff;
325
326
    }
  }
327
}