vel_control.c 8.43 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
10
11
12
13
14
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>

15
16
17
18
#include "pccom.h"
#include "vel_control.h"


19
// reference variables
20
21
volatile int32_t ref = 0;        // 11 frac bits
volatile int32_t refCtrl = 0;    // ref used in ctrl-loop (=ref sent from simulink)
22
23
24


// velocity control variables
25
26
27
28
29
30
31
32
33
34
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
35
36
volatile int32_t K = 807;       // 6 frac bits, prop constant
volatile int32_t Ke = 13;        // 6 frac bits, integral constant
37
volatile int8_t fr_comp = (10<<3);
Pontus Giselsson's avatar
Pontus Giselsson committed
38
39
#define V_MAX (120<<4)
#define V_MIN (-120<<4)
40

41
42

// encoder variables
43
#define ENCODERY  (PIND&0x04)        //Positional encoder pins
44
45
46
47
48
49
50
51
52
53
54
#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 posTemp = 0;    // position sent to simulink
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
55
56

// velocity estimation parameters
57
58
59
60
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)
61

62
// adc measurement variables
63
64
65
66
67
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) */
68
int32_t getPosition() {
69
  cli();
70
  posTemp = pos;
71
  sei();
72
  return ((int32_t) posTemp);
73
74
75
}


76
/* return velocity (in mm/s) */
77
int32_t getVelocity() {
78
79
80
  return velEstSend;
}

81
82

/* return last angle measurement */
83
84
85
int16_t getAngle() {
  low = inp(ADCL);
  high = inp(ADCH);
86
  return (((int16_t) ((high<<8) | low) - 512)-angleOffset);
87
88
}

89
90

/* return current-reference */
91
92
int32_t getCurrentRef() {
  return uSend;
93
94
}

95

96
/* Set new reference value */
97
98
99
100
void setRef(int32_t newRef) {
  ref = newRef;
}

101
102
103
104
105
106
107
/* Routine used to initialize the positional encoders */
void initPos()
{
  oldX = ENCODERX;
  oldY = ENCODERY;
}

108
/* Timer 2, Encoder counter, 73 kHz updates position variable */
109
SIGNAL(SIG_OUTPUT_COMPARE2) {
110

111
112
113
  // Update position from encoder
  newX = ENCODERX;
  newY = ENCODERY;
114
115
  if((newX != oldX) || (newY != oldY)) {                            //Check if any value changed
      /*
116
117
      sum = (oldX<<2)+oldY+newX+(newY>>2);  
      if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
118
	pos = pos+1;
119
      } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
120
121
122
123
124
	pos = pos-1;
      } else {
	brake = 1; // emergency brake
      }
      */
125
    // Works like if-clauses above, but faster!
126
127
128
129
      if ((oldX == newY) && (oldY != newX)) {
	pos = pos+1;
      } else if ((oldX != newY) && (oldY == newX)) {
	pos = pos-1;
130
131
132
133
134
      } else {
	brake = 1;
      }
      oldX = newX;
      oldY = newY;
135
136
137
138
139
  }

}


140
/* Timer 0, serial communication with simulink */
141
142
SIGNAL(SIG_OUTPUT_COMPARE0) {

143
144
145
  TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));   // Disable communication and ctrl-interrupts

  sei(); // enable interrupts from encoder-counter
Pontus Giselsson's avatar
Pontus Giselsson committed
146
  
147
  // Poll UART receiver
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
  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
163

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

166
  TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts
167

168
169
170
171
}



172
173
174
/* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE1A) {
  
175
  posCtrl = pos; // store pos to use in this loop
176

177
  sei(); // to enable interupts from encoder counter and communication
178

179
  // velocity estimation in mm/s
180
181
182
  velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos));  // 5 fracbits on velEst
  oldPos = posCtrl;
  
183
  // store velEst and ref to be sent/used here
184
185
186
  cli();
  velEstSend = velEst;
  refCtrl = ref;
187
  // ref = ref*(1-brake); // emergency stop
188
  sei();
189
190

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

193
194
  // temporary ctrl-signal
  v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
195
196

  // friction compensation
197
  if (refCtrl > 0) {
198
199
200
    v = v+fr_comp;
  } else if (refCtrl < 0) {
    v = v-fr_comp;
201
202
  }

203
204
205
206
207
208
209
210
211
212
213
214
215
216
  // 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;
217
  }
218
219
220
221
222
  
  if (intCond)
    I = I + (((Ke*e)+(1<<1))>>2);

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

225
  // u that is sent to simulink
226
227
228
  cli();
  uSend = u;
  sei();
229

Pontus Giselsson's avatar
Pontus Giselsson committed
230

231
  // TWI-communication to set current reference on the other atmel
232
233
234
  // send start command
  outp(BV(TWINT)|BV(TWEN)|BV(TWSTA),TWCR);
  while (!(TWCR&BV(TWINT))) {}
235
    
236
237
238
239
  // Contact slave  
  outp(0x02,TWDR);  // slave is 0x02 (sla+w)
  outp(BV(TWINT)|BV(TWEN),TWCR);
  while (!(TWCR&BV(TWINT))) {}
240
241
242
 
  // Send reference byte to slave
  outp((int8_t)u,TWDR);  // send 8 bits reference
243
244
  outp(BV(TWINT)|BV(TWEN),TWCR);
  while (!(TWCR&BV(TWINT))) {}
Pontus Giselsson's avatar
Pontus Giselsson committed
245
    
246
247
248
249
250
251
252
253
254
255
256
  // stop transmission
  outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);

}


int main()
{
  cli();
  
  //Port directions
257
  //outp(0x80,DDRB);   // Led output
258
  outp(0x10,DDRC);  // timer calculation port
259
260
261
  PORTD = 0x40;  // pull up on reset switch


262
  /* Timer section */
263
264
  // Enable timer0, timer1, timer2 compare match interrupts
  outp(BV(OCIE0)|BV(OCIE1A)|BV(OCIE2),TIMSK);
265
  
266
  /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
267
268
269
270
  outp(BV(WGM21)|BV(CS20),TCCR2);
  outp(200,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);
271
  
272
  /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
273
274
  outp(BV(WGM12)|BV(CS10),TCCR1B);
  outp(0x39,OCR1AH);
275
  outp(0x7f,OCR1AL);
276
277
278
279
  outp(0,TCNT1H);
  outp(0,TCNT1L);
  

Pontus Giselsson's avatar
Pontus Giselsson committed
280
  /* Timer 0, 40 kHz, Prescaler 8, serial communication */
281
  outp(BV(WGM01)|BV(CS01),TCCR0);
Pontus Giselsson's avatar
Pontus Giselsson committed
282
283
284
  //outp(184-1,OCR0); // 10 kHz
  outp(46-1,OCR0); // 40 kHz
   /* Reset timer 0 */
285
286
  outp(0,TCNT0);

287
288

  // syncronization (ctrl interrupt 34 micros before communication interrupt)
289
  TCNT1 = TCNT0*8+500;
290
291
  
  
292
  //Serial communication initialization
293
  outp(0x00, UCSRA);	// USART:
294
  outp(0x18, UCSRB);	// USART: RxEnable|TxEnable
295
296
  outp(0x86, UCSRC);	// USART: 8bit, no parity
  outp(0x00, UBRRH);	// USART: 115200 @ 14.7456MHz
297
  outp(7,UBRRL);	// USART: 115200 @ 14.7456MHz 
298
  
299
300
  
  /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
301
  outp(BV(REFS0)|BV(MUX0)|BV(MUX1),ADMUX);
302
  
303
  // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
304
  outp(BV(ADEN)|BV(ADSC)|BV(ADATE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSRA);
305
306
307
308
309
310
311
312


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

  // initialize position measurements
  initPos();
313

314
  // initialize pc-communication
315
  pccom_init();
316
317
318
319
320
  
  //Enable interrupts
  sei();

  // loop
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
  while (1) {
    // 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);
      //angleOffset = -150;
      sei();
      oldPos = 0;
      velEst = 0; // reset velocity estimate
      I = 0; // reset integral part of controller
      u = 0; // reset ctrl signal
    } 
  }
337
}