vel_control.c 12.6 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
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>
13
#include <compat/deprecated.h>
14

15
#include "vel_control.h"
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
#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;

46
47


48
// reference variables
49
volatile int32_t vel_ref = 0;    // 11 frac bits (variable in mm/s, max 2^12)
50
volatile int32_t refCtrl = 0;    // ref used in ctrl-loop (=ref sent from simulink)
51
volatile int32_t acc_ref = 0;    // 14 frac bits (variable in mm/s, max 2^15)
52
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
53
54

// velocity control variables
55
56
57
58
59
60
61
62
63
64
volatile int32_t u = 0;          // 11 frac bits
volatile int32_t uSend = 0;      // ctrl sent to simulink
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;


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

71
72

// encoder variables
73
#define ENCODERY  (PIND&0x04)        //Positional encoder pins
74
75
76
77
78
79
80
81
82
83
#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
84
85

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

91
// adc measurement variables
92
93
94
95
96
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) */
97
int32_t getPosition() {
98
  int16_t result = 0;
99
  cli();
100
  result = pos;
101
  sei();
102
  return (result);
103
104
105
}


106
/* return velocity (in mm/s) */
107
int32_t getVelocity() {
108
109
110
  return velEstSend;
}

111
112

/* return last angle measurement */
113
114
115
int16_t getAngle() {
  low = inp(ADCL);
  high = inp(ADCH);
116
117
  int16_t result = ADC - 512 -angleOffset;
  return result;
118
119
}

120
121

/* return current-reference */
122
123
int32_t getCurrentRef() {
  return uSend;
124
125
}

126

127
128
/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
129
  acc_ref = newAccRef;
130
131
132
133
134
135
  nbrSamples = 0;
}


/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
136
  vel_ref = newVelRef;
137
138
}

139
140
141
142
143
144
145
/* Routine used to initialize the positional encoders */
void initPos()
{
  oldX = ENCODERX;
  oldY = ENCODERY;
}

146
/* Timer 2, Encoder counter, 73 kHz updates position variable */
147
SIGNAL(TIMER2_COMP_vect) {
148
149
150
  // Update position from encoder
  newX = ENCODERX;
  newY = ENCODERY;
151
  if((newX != oldX) || (newY != oldY)) { //Check if any value changed
152
      /*
153
154
      sum = (oldX<<2)+oldY+newX+(newY>>2);  
      if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
155
	pos = pos+1;
156
      } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
157
158
159
160
161
	pos = pos-1;
      } else {
	brake = 1; // emergency brake
      }
      */
162
    // Works like if-clauses above, but faster!
163
164
165
166
      if ((oldX == newY) && (oldY != newX)) {
	pos = pos+1;
      } else if ((oldX != newY) && (oldY == newX)) {
	pos = pos-1;
167
168
169
170
171
      } else {
	brake = 1;
      }
      oldX = newX;
      oldY = newY;
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
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;
  }
}
216

217
#if 0
218
/* Timer 0, serial communication with simulink */
219
SIGNAL(TIMER0_COMP_vect) {
220

221
  TIMSK &= ~(_BV(OCIE0)|_BV(OCIE1A));   // Disable communication and ctrl-interrupts
222
223

  sei(); // enable interrupts from encoder-counter
Pontus Giselsson's avatar
Pontus Giselsson committed
224
  
225
  // Poll UART receiver
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
  uint8_t status = UCSRA;
  if (status & (1<<RXC)) {
    char ch = UDR;
    pccom_receiveByte(ch);
    
    if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) { 
      //main_emergencyStop(); // stop on USART error
    }     
  }
  
  // Poll UART sender
  if (UCSRA & (1<<UDRE)) {
    int16_t toSend = pccom_getNextByteToSend();
    if (toSend >= 0) UDR = (char)toSend;
  }
Pontus Giselsson's avatar
Pontus Giselsson committed
241

242
  TIFR = (1<<OCF0);        // skip pending interrupts from serial comm, (but not from ctrl)
243

244
  TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts
245

246
}
247
#endif
248
249


250
/* Timer 0, control loop , 1 kHz */
251
SIGNAL(TIMER1_COMPA_vect) {
252
  posCtrl = pos; // store pos to use in this loop
253

254
  sei(); // to enable interupts from encoder counter and communication
255

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

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

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

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

284
285
286
287
288
289
290
291
292
293
294
295
296
297
  // 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;
298
  }
299
300
301
302
303
  
  if (intCond)
    I = I + (((Ke*e)+(1<<1))>>2);

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

306
  // u that is sent to simulink
307
308
309
  cli();
  uSend = u;
  sei();
310

Pontus Giselsson's avatar
Pontus Giselsson committed
311

312
  // TWI-communication to set current reference on the other atmel
313
  // send start command
314
315
  outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTA),TWCR);
  while (!(TWCR&_BV(TWINT))) {}
316
    
317
318
  // Contact slave  
  outp(0x02,TWDR);  // slave is 0x02 (sla+w)
319
320
  outp(_BV(TWINT)|_BV(TWEN),TWCR);
  while (!(TWCR&_BV(TWINT))) {}
321
322
323
 
  // Send reference byte to slave
  outp((int8_t)u,TWDR);  // send 8 bits reference
324
325
  outp(_BV(TWINT)|_BV(TWEN),TWCR);
  while (!(TWCR&_BV(TWINT))) {}
Pontus Giselsson's avatar
Pontus Giselsson committed
326
    
327
  // stop transmission
328
  outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR);
329
330
331
332
333
334
335
336
}


int main()
{
  cli();
  
  //Port directions
337
  //outp(0xf0,DDRB);   // Led output
338
  outp(0x10,DDRC);  // timer calculation port
339
340
341
  PORTD = 0x40;  // pull up on reset switch


342
  /* Timer section */
343
  // Enable timer0, timer1, timer2 compare match interrupts
344
  outp(_BV(OCIE0)|_BV(OCIE1A)|_BV(OCIE2),TIMSK);
345
  outp(_BV(OCIE1A)|_BV(OCIE2),TIMSK);
346
  
347
  /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
348
  outp(_BV(WGM21)|_BV(CS20),TCCR2);
349
350
351
  outp(200,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);
352
  
353
  /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
354
355
356
  outp(_BV(WGM12)|_BV(CS10),TCCR1B);
  outp(0x38,OCR1AH);
  outp(0x3f,OCR1AL);
357
358
359
360
  outp(0,TCNT1H);
  outp(0,TCNT1L);
  

Pontus Giselsson's avatar
Pontus Giselsson committed
361
  /* Timer 0, 40 kHz, Prescaler 8, serial communication */
362
  outp(_BV(WGM01)|_BV(CS01),TCCR0);
Pontus Giselsson's avatar
Pontus Giselsson committed
363
  //outp(184-1,OCR0); // 10 kHz
364
  outp(45-1,OCR0); // 40 kHz
Pontus Giselsson's avatar
Pontus Giselsson committed
365
   /* Reset timer 0 */
366
367
  outp(0,TCNT0);

368
369

  // syncronization (ctrl interrupt 34 micros before communication interrupt)
370
  TCNT1 = TCNT0*8+500;
371
372
  
  
373
  //Serial communication initialization
374
  outp(0x00, UCSRA);	// USART:
375
  outp(0x18, UCSRB);	// USART: RxEnable|TxEnable
376
377
  outp(0x86, UCSRC);	// USART: 8bit, no parity
  outp(0x00, UBRRH);	// USART: 115200 @ 14.7456MHz
378
  outp(7,UBRRL);	// USART: 115200 @ 14.7456MHz 
379
380
381
382
383
384

  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
385
  
386
387
  
  /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
388
  outp(_BV(REFS0)|_BV(MUX0)|_BV(MUX1),ADMUX);
389
  
390
  // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
391
  outp(_BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0),ADCSRA);
392
393
394
395


  // Initialize Master TWI
  outp(0x10,TWBR);  // set SCL-frequency CPU-freq/(16+2*16)
396
  outp(_BV(TWEN),TWCR); // enable TWI
397
398
399

  // initialize position measurements
  initPos();
400

401
  // initialize pc-communication
402
403
  serialio_init();
  pccom_poll = 0;
404
405
406
407
408
  
  //Enable interrupts
  sei();

  // loop
409
  while (1) {
410
    unsigned char to_poll;
411
412
413
414
415
416
417
418
419
420
421
    // reset position, velocity estimate and integral part of ctrl if reset button pushed
    if (!(PIND & 0x40)) {
      cli();
      low = inp(ADCL);
      high = inp(ADCH);
      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
422
      sei();
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
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
    }
    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();
    }
467
  }
468
}