vel_control.c 7.65 KB
Newer Older
1
2
3
4
5
6
7
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>


8
9
10
11
#include "pccom.h"
#include "vel_control.h"


12
13
// reference variables
volatile int32_t ref = 0;  // 11 frac bits
14
volatile int32_t refCtrl = 0;
15
16
17
18


// velocity control variables
volatile int32_t u = 0;  // 11 frac bits
19
volatile int32_t uSend = 0;
20
21
22
23
24
25
26
27
28
volatile int32_t v = 0;  // 11 frac bits
volatile int8_t brake = 0;
volatile int32_t I = 0; // 11 frac bits
volatile int32_t e = 0; // 11 frac bits
volatile int32_t K = 1200; // 6 frac bits
volatile int32_t Ke = 45; // 6 frac bits
volatile int32_t Ksat = 3; // 6 frac bits

// encoder variables
29
30
31
32
33
//#define ENCODERY  (PIND&(uint8_t)(1<<2))        //Positional encoder pins
//#define ENCODERX  (PINB&(uint8_t)(1<<1))        //Positional encoder pins
#define ENCODERY  (PIND&0x04)        //Positional encoder pins
#define ENCODERX  ((PINB&0x02)<<1)        //Positional encoder pins

34
volatile int16_t pos = 0;
35
volatile int16_t posTemp = 0;
36
volatile int16_t posCtrl = 0;
37
38
39
40
41
42
43
44
45
volatile int16_t oldPos = 0;
volatile int8_t newX;
volatile int8_t newY;
volatile int8_t oldX;
volatile int8_t oldY;
volatile int8_t sum = 0;

// velocity estimation parameters
volatile int32_t velEst = 0; // 5 frac bits
46
47
//volatile int32_t velEstTemp = 0; 
volatile int32_t velEstSend = 0;
48
49
50
int16_t a = 116; //7 frac bits
int16_t b = 152; // 5 frac bits

51
52
53
// adc measurement variables
volatile int16_t low;
volatile int16_t high;
54

55
56
// return position (in tics) 
int32_t getPosition() {
57
  cli();
58
  posTemp = pos;
59
  sei();
60
  return ((int32_t) posTemp);
61
62
63
64
65
}


// return velocity (in mm/s) 
int32_t getVelocity() {
66
67
68
69
70
71
72
73
74
75
76
77
78
  return velEstSend;
}

// return last angle measurement
int16_t getAngle() {
  low = inp(ADCL);
  high = inp(ADCH);
  return ((int16_t) ((high<<8) | low) - 512);
}

// return current reference
int32_t getCurrentRef() {
  return uSend;
79
80
}

81

82

83
84
85
86
87
88
89
/* Routine used to set the red LED */
void setLED(uint8_t on)
{
  if (on) PORTB &= ~0x80; //Turn on
  else PORTB |= 0x80;     //Turn off
}

90
91
92
93
94
95
// Set new reference value
void setRef(int32_t newRef) {
  ref = newRef;
}


96
97
98
99
100
101
102
/* Routine used to initialize the positional encoders */
void initPos()
{
  oldX = ENCODERX;
  oldY = ENCODERY;
}

103
104
/* Routine used to reset the cart position */
void resetPos()
105
{
106
  
107
108
}

109
/* Timer 2, Encoder counter, 73 kHz */
110
SIGNAL(SIG_OUTPUT_COMPARE2) {
111

112
  
113

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

139
  
140
141
142
143
144
145
146

}


/* Timer 1, serial communication */
SIGNAL(SIG_OUTPUT_COMPARE0) {

147
  TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
148

149
150
  sei(); // enable interrupts from timer 0 and timer 2  
    PORTC |= 0x10;  // to clock calulation time
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
  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;
  }
166
  
167
168
169
170
171
172
  PORTC &= ~0x10;   
  TIFR = (1<<OCF0);
  //TIFR = (1<<OCF1A);

  TIMSK |= (BV(OCIE0)|BV(OCIE1A)); 

173
174
175
176
}



177
178
179
/* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE1A) {
  
180
  TIMSK &= ~(BV(OCIE1A));
181

182
183
184
  posCtrl = pos; // to aviod overwrite while reading

  sei(); // to enable interupts from timer2
185

186
 
187
  // linear velocity estimator
188
189
190
191
192
193
194
195
196
197
198
  // velocity estimate in mm/s
  velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos));  // 5 fracbits on velEst
  oldPos = posCtrl;

  // emergency stop
  ref = ref*(1-brake);
  
  cli();
  velEstSend = velEst;
  refCtrl = ref;
  sei();
199
200

  // control error
201
  e = refCtrl-((velEst+16)>>5);  // mm/s
202
203
204
205
206
207
208
209
210
  
  v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));

  //saturation and update integral part of ctrl
  if (v > 2047) {
    I = I + ((((Ke*e) + (Ksat)*(2047-v))+(1<<1))>>2);
  } else if (v < -2048) {
    I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2);
  } else {
211
    I = I + ((Ke*e+(1<<1))>>2);
212
213
214
215
216
  }
  
  u = (v+8)>>4; //8 frac bits to current loop

  // friction compensation
217
  if (refCtrl > 0) {
218
219
220
221
222
223
224
225
226
227
228
229
    u = u+10;
  } else if (ref < 0) {
    u = u-10;
  }

  // Saturation
  if (u > 127) {
    u = 127;
  } else if (u < -128) {
    u = -128;
  }

230
231
232
  cli();
  uSend = u;
  sei();
233

234
  
235
  // reset position, velocity estimate and integral part of ctrl if reset button pushed
236
  
237
238
239
240
241
242
243
244
  if (!(PIND & 0x40)) {
    cli();
    pos = 0; // reset position
    sei();
    oldPos = 0;
    velEst = 0; // reset velocity estimate
    I = 0; // reset integral part of controller
    u = 0; // reset ctrl signal
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
  }
  
  // TWI-communication
  // send start command
  outp(BV(TWINT)|BV(TWEN)|BV(TWSTA),TWCR);
  while (!(TWCR&BV(TWINT))) {}
  
  
  
  // Contact slave  
  outp(0x02,TWDR);  // slave is 0x02 (sla+w)
  outp(BV(TWINT)|BV(TWEN),TWCR);
  while (!(TWCR&BV(TWINT))) {}
  
   
  // Send reference byte
  outp((int8_t)(u&0x000000ff),TWDR);  // send 8 bits reference
  outp(BV(TWINT)|BV(TWEN),TWCR);
  while (!(TWCR&BV(TWINT))) {}
  
  
  // stop transmission
  outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);

269
 
270

271
272
  //TIFR = (1<<OCF0); 
  TIFR = (1<<OCF1A); 
273

274
  TIMSK |= (BV(OCIE1A));
275

276
277
278
279
280
281
282
283
284
285
286
287
}


int main()
{
  cli();
  
  //Port directions
  outp(0x80,DDRB);   // Led output
  outp(0x10,DDRC);  // timer calculation port

  /* Timer section */
288
289
  // Enable timer0, timer1, timer2 compare match interrupts
  outp(BV(OCIE0)|BV(OCIE1A)|BV(OCIE2),TIMSK);
290
  
291
  /* Timer 2, 73 kHz Prescaler 1 */
292
293
294
295
  outp(BV(WGM21)|BV(CS20),TCCR2);
  outp(200,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);
296
297
298
299
300
301
302
303
304
305
  
  // --------old-----------------------------------------------
  /* Timer 1, Prescaler 1, 73/8 kHz*/
  
  //outp(BV(WGM12)|BV(CS10),TCCR1B);
  //outp(0x06,OCR1AH);
  //outp(0x40,OCR1AL);
  //outp(0,TCNT1H);
  //outp(0,TCNT1L);
  
306
307

  /* Timer 0, 1 kHz Prescaler 64 */
308
309
310
311
312
313
314
315
316
317
318
  
  //outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
  //outp(230,OCR0);
  
  /* Reset timer 0 */
  //outp(0,TCNT0);
  
  // --------new-----------------------------------------------
  /* Timer 1, 1 kHz */
  outp(BV(WGM12)|BV(CS10),TCCR1B);
  outp(0x39,OCR1AH);
319
  outp(0x7f,OCR1AL);
320
321
322
323
  outp(0,TCNT1H);
  outp(0,TCNT1L);
  

324
325
  /* Timer 0, 10 kHz, Prescaler 8 */
  outp(BV(WGM01)|BV(CS01),TCCR0);
326
  //outp(200,OCR0);
327
  outp(184-1,OCR0); // 10 kHz
328

329
330
  /* Reset timer 0 */
  outp(0,TCNT0);
331
  //----------------------------------------------------------
332

333
334
  // syncronization (timer1 interrupt 34 micros before timer0 interrupt)
  TCNT1 = TCNT0*8+500;
335
336
  
  
337
338
  //Serial communication
  outp(0x00, UCSRA);	// USART:
339
  outp(0x18, UCSRB);	// USART: RxEnable|TxEnable
340
341
342
343
344
345
346
  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 */
347
  outp(BV(REFS0)|BV(MUX0)|BV(MUX1),ADMUX);
348
  
349
350
  // Enable ADC on mux3, start first conversion, prescaler 128, free running mode
  outp(BV(ADEN)|BV(ADSC)|BV(ADATE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSRA);
351
352
353
354
355
356
357
358


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

  // initialize position measurements
  initPos();
359
360

  pccom_init();
361
362
363
364
365
366
367
  
  //Enable interrupts
  sei();

  // loop
  while (1) {}
}