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


// control variables
9
volatile int16_t ref=0;   // reference value (from velocity controller)
10
volatile unsigned char lbyte,hbyte;  
11
volatile int16_t y;       // measurement, 9 frac bits
12
volatile uint8_t low, high;
13
14
15
16
volatile int16_t e = 0;   // control error, 9 frac bits
volatile int16_t v = 0;   // temporary ctrl signal, 9 frac bits
volatile int16_t I = 0;   // integral part of ctrl, 13 frac bits
volatile int16_t u = 0;   // ctrl signal = pwm high time
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
volatile int16_t K = 74; // 7 frac bits
volatile int16_t Ke = 26; //7 frac bits, K*h/Ti
volatile int16_t Ksat = 44; // 7 frac bits, h/Tr


// 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];
volatile int16_t skipSamples = 1000;
volatile int16_t countSamples = 0;
volatile int16_t jj=0;
volatile int8_t stop = 0;

// twi variables
volatile int16_t status;


36
37
38
39
/* Send logged data over Serial connection */
static inline void sendData() {
  int16_t ii = 0;
  while (ii < log_len) {
40

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

44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
    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++;
  }
}




/* Timer 2 compare match interupt */
62
63
64
65
66
67
68
69
70
SIGNAL(SIG_OUTPUT_COMPARE2) {
  // Start AD conversion
  ADCSRA |= BV(ADSC);
  
  // Read input
  low = inp(ADCL);
  high = inp(ADCH);
  y =  ((high<<8) | low) - 512; //y 9 frac bits
  
71
  //control error, (+) since negative measurements
72
73
74
  e = ref+y; //e 9 frac bits
  
  v = (int16_t)(((K*e+64)>>7)+(I>>4));
75

76
77
78
79
80
81
82
83
84
  //saturation and update integral part of ctrl
  if (v > 511) {
    I = I + (((Ke*e) + (Ksat)*(511-v))>>3);
  } else if (v < -512) {
    I = I + (((Ke*e) + (Ksat)*(-512-v))>>3);
  } else {
    I = I + ((Ke*e)>>3);
  }
  
85
  // ctrl signal, 7 bits + direction
86
87
  u = (v+2)>>2; //7 frac bits to pwm 
  
88
  // saturation of ctrl-signal
89
90
91
92
93
  if (u > 127) {
    u = 127;
  } else if (u < -128) {
    u = -128;
  }
94

95
96
97
98
99
100
101
102
103
  // set pwm switching time
  if (u < 0) {
    PORTC |= 0x80;
    OCR1BL = (unsigned char) (128-(-u));
  } else {
    PORTC = (PORTC & 0x7F);
    OCR1BL = (unsigned char) (127-u);
  }

104
  // TWI-communication, recieve reference from velocity controller
105
106
107
108
109
110
111
112
113
114
115
  if ((BV(TWINT)&inp(TWCR))) {
    status = (inp(TWSR)&0xf8);
    if (status == 0x80) {
      ref = (int16_t)((int8_t)(inp(TWDR))); // read reference
      ref = ref << 2;  // shifted 2 steps for 10 bits for reference
    } 
    else {
    }
    outp(BV(TWINT)|BV(TWEN)|BV(TWEA),TWCR);
  }

116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
  // For logging purposes
  /*
  countSamples++;
  if (countSamples == skipSamples) {
    ctrl_log[jj] = u;
    I_log[jj] = I;
    error_log[jj] = e;
    jj++;
    countSamples = 0;
    
    // Stop after a while
    if ((jj == (log_len-1)) & !stop) {
	outp(0x7f,OCR1BL);
	stop = 1;
	sendData();
    }
  }
  */
134
135
136
137
138
139

}


int main()
{
140
  // clear interrupts
141
142
143
144
145
  cli();
  
  //Port directions
  outp(0x08,PORTC); // pull up on overtemp signals
  outp(0xa0,DDRC);  // output on dir and brake
146
  outp(0x10,DDRD);  // output on pwm
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
  
  /* Timer section */
  
  // 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);
  
  // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> h_pwm=8.61 micros)
  outp(0x00,OCR1AH);
  outp(0x7f,OCR1AL);
  outp(0x00,TCNT1H);
  outp(0x00,TCNT1L);
  outp(0x00,OCR1BH);
  outp(0x7f,OCR1BL); // to not start motor-rotation before control

  
  /* Timer 2, 4 times pwm-period, for control sampling, prescaler 8 */
  outp(BV(WGM21)|BV(CS21),TCCR2);
  outp(0x3f,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);
168
169
170
171

  // Enable timer2 compare match interrupts
  outp(BV(OCIE2),TIMSK);

172
173
  //Serial communication
  outp(0x00, UCSRA);	// USART:
174
  outp(0x18, UCSRB);	// USART: RxIntEnable|RxEnable|TxEnable
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
  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 */
  outp(BV(REFS0)|BV(MUX0),ADMUX); 	
  
  // Enable ADC, start first conversion, prescaler 32, not free running mode
  outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);


  // Initialize TWI
  outp(0x02,TWBR);  // set SCL-frequency CPU-freq/(16+2*2)
  outp(0x02,TWAR);  // slave address
  outp(BV(TWEA)|BV(TWEN),TWCR); // enable TWI, enable ACK

  //Enable interrupts
  sei();

  // loop
  while (1) {}
}