vel_control.c 11.9 KB
Newer Older
Pontus Giselsson's avatar
Pontus Giselsson committed
1
2
/*
**************************************************************
3
4
5
6

 Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
           for LTH - reglerteknik

Pontus Giselsson's avatar
Pontus Giselsson committed
7
8
***************************************************************
*/
9
#include <util/twi.h>
10
11
12
13
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>

14
#include "vel_control.h"
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
#include "serialio.h"

/*
 * Serial I/O assignments
 *
 *   AO 0 -- Axis 1  motor voltage
 *   A0 1 -- Axis 1  velocity reference !
 *
 *   EI 0 -- Axis 1  position !
 *   EI 1 -- Axis 1  filtered velocity !
 *   EI 5 -- Axis 1  position with predicted fraction
 *   EI 6 -- Axis 1  position unquantized
 *
 *   AI 2 -- Axis 1  current
 *   AI 3 -- Pendulum angle !
 *
 *   AI 4 -- Axis 1  motor voltage (actual)
 *
 *   DI 0 -- Axis 1  endpoint sensor
 */

#define POLL_AXIS1_POSITION      0x01
#define POLL_AXIS1_VELOCITY      0x02
#define POLL_PEND_ANGLE          0x04
#define POLL_CURRENT_REFERENCE   0x08
//#define POLL_AXIS1_RESET         0x0008
#define POLL_CONFIG              0x80

static volatile uint8_t pccom_poll=0;

45

Anders Blomdell's avatar
Anders Blomdell committed
46
47
// Keep alive timer
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
48

49
// reference variables
Anders Blomdell's avatar
Anders Blomdell committed
50
volatile int32_t g_vel_ref = 0;  // 11 frac bits (variable in mm/s, max 2^12)
51
volatile int32_t refCtrl = 0;    // ref used in ctrl-loop (=ref sent from simulink)
Anders Blomdell's avatar
Anders Blomdell committed
52
volatile int32_t g_acc_ref = 0;  // 14 frac bits (variable in mm/s, max 2^15)
53
54

// velocity control variables
55
56
57
58
59
60
61
62
63
volatile int32_t u = 0;          // 11 frac bits
volatile int32_t v = 0;          // 11 frac bits
volatile int32_t vSat = 0;
volatile int8_t brake = 0;       // brake variable if pos-sample missed
volatile int32_t I = 0;          // 11 frac bits
volatile int32_t e = 0;          // 11 frac bits
volatile int8_t intCond = 0;


Anders Blomdell's avatar
Anders Blomdell committed
64
volatile int32_t K = 807;        // 6 frac bits, prop constant
Pontus Giselsson's avatar
Pontus Giselsson committed
65
volatile int32_t Ke = 13;        // 6 frac bits, integral constant
66
volatile int8_t fr_comp = (10<<3);
Pontus Giselsson's avatar
Pontus Giselsson committed
67
68
#define V_MAX (120<<4)
#define V_MIN (-120<<4)
69

70
71

// encoder variables
72
#define ENCODERY  (PIND&0x04)        //Positional encoder pins
73
74
75
76
77
78
79
80
81
82
#define ENCODERX  ((PINB&0x02)<<1)   //Positional encoder pins (shift one step for faster comparison with Y)

// position variables
volatile int16_t pos = 0;        // position in tics
volatile int16_t posCtrl = 0;    // position used in ctrl-loop
volatile int16_t oldPos = 0;     // previous pos used for velocity estimation
volatile int8_t newX;            // encoder signal
volatile int8_t newY;            // encoder signal
volatile int8_t oldX;            // previous encoder signal
volatile int8_t oldY;            // previous encoder signal
83
84

// velocity estimation parameters
85
86
87
volatile int32_t velEst = 0;     // vel-estimate, 5 frac bits
int16_t a = 116;                 // 7 frac bits (parameter in velocity estimate)
int16_t b = 152;                 // 5 frac bits (parameter in velocity estimate)
88

89
// adc measurement variables
90
91
92
93
94
volatile int16_t low;            // temporary variable for ad-reading
volatile int16_t high;           // temporary variable for ad-reading
volatile int16_t angleOffset = 0;

/* return position (in tics) */
95
int32_t getPosition() {
96
  int16_t result = 0;
97
  cli();
98
  result = pos;
99
  sei();
100
  return (result);
101
102
103
}


104
/* return velocity (in mm/s) */
105
int32_t getVelocity() {
Anders Blomdell's avatar
Anders Blomdell committed
106
107
108
109
110
  int32_t result;
  cli();
  result = velEst;
  sei();
  return result;
111
112
}

113
114

/* return last angle measurement */
115
int16_t getAngle() {
Anders Blomdell's avatar
Anders Blomdell committed
116
117
118
119
120
  int16_t adc = ADC;
  while (adc != ADC) {
    adc = ADC;
  }
  return adc - 512 - angleOffset;
121
122
}

123
124

/* return current-reference */
125
int32_t getCurrentRef() {
Anders Blomdell's avatar
Anders Blomdell committed
126
127
128
129
130
  int32_t result;
  cli();
  result = u;
  sei();
  return result;
131
132
}

133

134
135
/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
Anders Blomdell's avatar
Anders Blomdell committed
136
  g_acc_ref = newAccRef;
137
138
139
140
141
142
  nbrSamples = 0;
}


/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
Anders Blomdell's avatar
Anders Blomdell committed
143
  g_vel_ref = newVelRef;
144
145
}

146
147
148
149
150
151
152
/* Routine used to initialize the positional encoders */
void initPos()
{
  oldX = ENCODERX;
  oldY = ENCODERY;
}

153
/* Timer 2, Encoder counter, 73 kHz updates position variable */
154
SIGNAL(TIMER2_COMP_vect) {
155
156
157
  // Update position from encoder
  newX = ENCODERX;
  newY = ENCODERY;
158
  if((newX != oldX) || (newY != oldY)) { //Check if any value changed
Anders Blomdell's avatar
Anders Blomdell committed
159
    /*
160
161
      sum = (oldX<<2)+oldY+newX+(newY>>2);  
      if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
Anders Blomdell's avatar
Anders Blomdell committed
162
      pos = pos+1;
163
      } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
Anders Blomdell's avatar
Anders Blomdell committed
164
      pos = pos-1;
165
      } else {
Anders Blomdell's avatar
Anders Blomdell committed
166
      brake = 1; // emergency brake
167
      }
Anders Blomdell's avatar
Anders Blomdell committed
168
    */
169
    // Works like if-clauses above, but faster!
Anders Blomdell's avatar
Anders Blomdell committed
170
171
172
173
174
175
176
177
178
    if ((oldX == newY) && (oldY != newX)) {
      pos = pos+1;
    } else if ((oldX != newY) && (oldY == newX)) {
      pos = pos-1;
    } else {
      brake = 1;
    }
    oldX = newX;
    oldY = newY;
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
SIGNAL(USART_RXC_vect) {
  char ch = UDR;
  switch (serialio_RXC(ch)) {
    case serialio_clearbit: {
      switch (serialio_channel) {
      }
    } break;
    case serialio_setbit: {
      switch (serialio_channel) {
      }
    } break;
    case serialio_pollbit: {
      switch (serialio_channel) {
        //case 0: { addPoll(POLL_AXIS1_RESET); } break;
      }
    } break;
    case serialio_pollchannel: {
      switch (serialio_channel) {
        case 0:  pccom_poll |= POLL_AXIS1_POSITION;    break;
        case 1:  pccom_poll |= POLL_AXIS1_VELOCITY;    break;
        case 2:  pccom_poll |= POLL_PEND_ANGLE;        break;
        case 3:  pccom_poll |= POLL_CURRENT_REFERENCE; break;
        case 31: pccom_poll |= POLL_CONFIG;            break;
      }
    } break;
    case serialio_setchannel: {
      switch (serialio_channel) {
        case 0: { 
          setVelRef(serialio_value - (1L<<12));
        } break;
        case 1: {
          setAccRef(serialio_value - (1L<<15));
        } break;
      }
    } break;
    case serialio_more: {
    } break;
    case serialio_error: {
    } break;
  }
}
223

Anders Blomdell's avatar
Anders Blomdell committed
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
SIGNAL(TWI_vect) {
  unsigned char twsr = TWSR;
  switch (twsr) {
    case 0x08: {
      TWDR = 0x02; // slave is 0x02 (sla+w)
      TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
    } break;
    case 0x18:
    case 0x20: {
      TWDR = u;
      TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
    } break;
    case 0x28:
    case 0x30: {
      TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWSTO)|_BV(TWIE);
    } break;
    default: {
      // This should never happen
      TWCR = (_BV(TWINT)|_BV(TWEN)|_BV(TWIE));
    } break;
244
  }
245
246
}

247
/* Timer 0, control loop , 1 kHz */
248
SIGNAL(TIMER1_COMPA_vect) {
249
  posCtrl = pos; // store pos to use in this loop
Anders Blomdell's avatar
Anders Blomdell committed
250
251
252
253
254
255
  int32_t vel_ref = g_vel_ref;
  int32_t acc_ref = g_acc_ref;
  if (nbrSamples < 65535) {
    nbrSamples++;
  }
  sei(); // to enable interupts from encoder counter and TWI
256

257
  // velocity estimation in mm/s
258
259
260
  velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos));  // 5 fracbits on velEst
  oldPos = posCtrl;
  
261
  // store velEst and ref to be sent/used here
Anders Blomdell's avatar
Anders Blomdell committed
262
//  cli();
263
  refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10);  //shift nbrSamples 10 steps (= nbrSamples*h)
264
265
266
  if (nbrSamples > 500) {
    refCtrl = 0;   // for safety reasons if doesn't send anything in 0.5 s
  }
267
  // vel_ref = vel_ref*(1-brake); // emergency stop
268
269

  // control error
270
  e = refCtrl-((velEst+16)>>5);  // mm/s
271

272
273
  // temporary ctrl-signal
  v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
274
275

  // friction compensation
276
  if (refCtrl > 0) {
277
278
279
    v = v+fr_comp;
  } else if (refCtrl < 0) {
    v = v-fr_comp;
280
281
  }

282
283
284
285
286
287
288
289
290
291
292
293
294
295
  // variable that decides if I-part should be updated
  intCond = 1;
  
  // saturation of v
  if (v > V_MAX) {
    vSat = V_MAX;
    if (e > 0)
      intCond = 0;
  } else if (v < V_MIN) {
    vSat = V_MIN;
    if (e < 0)
      intCond = 0;
  } else {
    vSat = v;
296
  }
297
298
299
300
  
  if (intCond)
    I = I + (((Ke*e)+(1<<1))>>2);

Anders Blomdell's avatar
Anders Blomdell committed
301
302
303
  // Protect u and TWCR for concurrent updates
  cli();

304
  // scale ctrl-signal to send over twi
Pontus Giselsson's avatar
Pontus Giselsson committed
305
  u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
306

Pontus Giselsson's avatar
Pontus Giselsson committed
307

308
  // TWI-communication to set current reference on the other atmel
309
  // send start command
Anders Blomdell's avatar
Anders Blomdell committed
310
311
312
  if (TWCR == (_BV(TWEN)|_BV(TWIE)) || TWCR == 0) {
    TWCR = _BV(TWEN)|_BV(TWSTA)|_BV(TWIE);
  }
313
314
315
316
317
318
319
320
}


int main()
{
  cli();
  
  //Port directions
Anders Blomdell's avatar
Anders Blomdell committed
321
  PORTC = 0x03;  // Pull up for systems without TWI resistors
322
323
324
  PORTD = 0x40;  // pull up on reset switch


325
  /* Timer section */
Anders Blomdell's avatar
Anders Blomdell committed
326
327
  // Enable timer1, timer2 compare match interrupts
  TIMSK = _BV(OCIE1A)|_BV(OCIE2);
328
  
329
  /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
Anders Blomdell's avatar
Anders Blomdell committed
330
331
332
  TCCR2 = _BV(WGM21)|_BV(CS20);
  OCR2 = 200;

333
  /* Reset timer 2 */
Anders Blomdell's avatar
Anders Blomdell committed
334
  TCNT2 = 0;
335
  
336
  /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
Anders Blomdell's avatar
Anders Blomdell committed
337
338
339
340
341
  TCCR1B = _BV(WGM12)|_BV(CS10);
  OCR1AH = 0x38;
  OCR1AL = 0x3f;
  TCNT1H = 0;
  TCNT1L = 0;
342
343
  

Pontus Giselsson's avatar
Pontus Giselsson committed
344
  /* Timer 0, 40 kHz, Prescaler 8, serial communication */
Anders Blomdell's avatar
Anders Blomdell committed
345
346
347
  TCCR0 = _BV(WGM01)|_BV(CS01);
  //OCR0 = 184-1; // 10 kHz
  OCR0 = 45-1; // 40 kHz
Pontus Giselsson's avatar
Pontus Giselsson committed
348
   /* Reset timer 0 */
Anders Blomdell's avatar
Anders Blomdell committed
349
  TCNT0 = 0;
350

351
352

  // syncronization (ctrl interrupt 34 micros before communication interrupt)
353
  TCNT1 = TCNT0*8+500;
354
355
  
  
356
  //Serial communication initialization
Anders Blomdell's avatar
Anders Blomdell committed
357
358
359
360
361
  UCSRA = 0x00;	// USART:
  UCSRB = 0x18;	// USART: RxEnable|TxEnable
  UCSRC = 0x86;	// USART: 8bit, no parity
  UBRRH = 0x00;	// USART: 115200 @ 14.7456MHz
  UBRRL = 7;	// USART: 115200 @ 14.7456MHz 
362
363
364
365
366
367

  UCSRA = 0x00;     // USART: 
  UCSRB = 0x98;     // USART: RxIntEnable|RxEnable|TxEnable 
  UCSRC = 0x86;     // USART: 8bit, no parity, 9600 
  UBRRH = 0x0;      // USART: 115200 @ 14.7456MHz
  UBRRL = 7;        // USART: 115200 @ 14.7456MHz
368
  
369
370
  
  /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
Anders Blomdell's avatar
Anders Blomdell committed
371
  ADMUX = _BV(REFS0)|_BV(MUX0)|_BV(MUX1);
372
  
373
  // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
Anders Blomdell's avatar
Anders Blomdell committed
374
  ADCSRA = _BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
375
376
377


  // Initialize Master TWI
Anders Blomdell's avatar
Anders Blomdell committed
378
379
  TWBR = 0x10;  // set SCL-frequency CPU-freq/(16+2*16)
  TWCR = _BV(TWEN)|_BV(TWIE);
380
381
382

  // initialize position measurements
  initPos();
383

384
  // initialize pc-communication
385
386
  serialio_init();
  pccom_poll = 0;
387
388
389
  
  //Enable interrupts
  sei();
Anders Blomdell's avatar
Anders Blomdell committed
390
  
391
392

  // loop
393
  while (1) {
394
    unsigned char to_poll;
395
396
397
    // reset position, velocity estimate and integral part of ctrl if reset button pushed
    if (!(PIND & 0x40)) {
      cli();
Anders Blomdell's avatar
Anders Blomdell committed
398
399
      low = ADCL;
      high = ADCH;
400
401
402
403
404
405
      pos = 0; // reset position
      angleOffset =  ((int16_t) ((high<<8) | low) - 512);
      oldPos = 0;
      velEst = 0; // reset velocity estimate
      I = 0; // reset integral part of controller
      u = 0; // reset ctrl signal
Pontus Giselsson's avatar
Pontus Giselsson committed
406
      sei();
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
    }
    cli();
    to_poll = pccom_poll;
    pccom_poll = 0;
    sei();    
    if (to_poll & POLL_AXIS1_POSITION) {
      serialio_putchannel(0, getPosition()+(1L<<15)); 
    }
    if (to_poll & POLL_AXIS1_VELOCITY) {
      serialio_putchannel(1, getVelocity()+(1L<<17)); 
    }
    if (to_poll & POLL_PEND_ANGLE) {
      serialio_putchannel(2, getAngle()+(1L<<10)); 
    }
    if (to_poll & POLL_CURRENT_REFERENCE) {
      serialio_putchannel(3, getCurrentRef()+(1L<<7)); 
    }
    if (to_poll & POLL_CONFIG) {
      CONF_ANALOG_IN(0, CONF_RESOLUTION(16));     // Position (now reference)
      CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_IN(1, CONF_RESOLUTION(18));     // Velocity estimate
      CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_IN(2, CONF_RESOLUTION(11));     // Pend angle measurement
      CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_IN(3, CONF_RESOLUTION(8));     // Current reference
      CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_OUT(0, CONF_RESOLUTION(13));    // Reference to vel-ctrl
      CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_OUT(1, CONF_RESOLUTION(16));    // Reference to acc-ctrl
      CONF_ANALOG_OUT(1, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_OUT(1, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_END();
    }
451
  }
452
}