current_control.c 5.01 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
/*********************************************

 Current regulation - Aleks Ponjavic 18/09/07
           for LTH - reglerteknik

   Note that the voltage is actually a duty
   cycle for the PWM. The PWM is running at
  12V with a duty cycle set by a 10bit value

*********************************************/

//#include <math.h>		//Only include if sin will be used, takes up memory!
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>

18
19
20

// reference generation variables
volatile int16_t ref = 200;
21
22
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
23
24
25
26
27
28
29
30
31



// control variables
volatile unsigned char lbyte,hbyte;  
volatile int16_t result;
volatile int16_t ctrl; 
volatile int16_t ctrl_out;
volatile int16_t temp;
32
33
volatile uint8_t alt = 1;

34
35
36
volatile uint8_t low, high;


37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

/* Routine used to set the red LED */
void setLED(uint8_t on)
{
  if (on) PORTB &= 0x7f;	//Turn on
  else PORTB |= 0x80;	//Turn off
}

/* Routine used to set pin PD7 */
void setPA4(uint8_t on)
{
  if (on == 0) PORTA &= 0xef;	//Turn off
  else PORTA |= 0x10;	//Turn on
}

/* Routine used to transmit bytes on the serial port */
static void putchar(unsigned char ch)
{
	while ((inp(UCSRA) & 0x20) == 0) {};
	outp(ch, UDR);
	while ((inp(UCSRA) & 0x20) == 0) {};
}

/* Interrupt service routine for handling incoming bytes on the serial port 
 might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}
63
64


65
66
67
68
69
70
71
72
static inline int16_t readInput() {
  uint8_t low, high;
  ADCSRA |= 0x40;
  while (ADCSRA & 0x40);
  low = ADCL;
  high = ADCH;
  return ((high<<8) | low) - 512;
}
73
	
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

/* Write 8-bit output using the PWM-generator */
static inline void writeOutput(int16_t val) {
  if (val < 0) {
    PORTC = 0x80+(PORTC & 0x7F);
    OCR1BH = 0;    //(unsigned char) (-val)&0xff00;
    OCR1BL = (unsigned char) (-val)&0x00ff;
  } else {
    PORTC = (PORTC & 0x7F);
    OCR1BH = 0;    //(unsigned char) (val&0xff00);
    OCR1BL = (unsigned char) (val&0x00ff);
  }
}


89
90
91

/* Interrupt when AD-conversion completes */
SIGNAL(SIG_ADC)
92
{
93
  setPA4(1);
94

95
96
97
98
  // Read input
  low = inp(ADCL);
  high = inp(ADCH);
  result =  ((high<<8) | low) - 512;
99
  
100
  // Scale incoming current measurement
101
102
  result = result*3; //!!!!!!!!!!!!!!!!!!!!!!!

103
  // Scale incoming current measurement
104
  // result += 13;
105
106
    
  //control
107
108
109
110
  ctrl = ref-result;
  ctrl = ((ctrl + (1<< 2)) >> 3);
  
  //saturation/rounding to 8 bit
111
112
113
114
  if (ctrl > 511)
    ctrl_out = 511;
  else if (ctrl < -511)
    ctrl_out = -511;
115
116
  else
    ctrl_out = ctrl;
117
118
119
120
121
122
123
124
125
126
127


  // write output
  if (ctrl_out < 0) {
    PORTC = 0x80+(PORTC & 0x7F);
    OCR1BL = (unsigned char) (-ctrl_out);
  } else {
    PORTC = (PORTC & 0x7F);
    OCR1BL = (unsigned char) (ctrl_out);
  }
  setPA4(0);
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
}


/* reference square- or triangle wave generator with timer 0 */
SIGNAL(SIG_OVERFLOW0) {
  int8_t rectangle = 1;
  refCount++;
  if (rectangle == 1) {
    if (refFlag == 0) {
      if (refCount == 10) {
	refFlag = 1;
	ref = -ref;
	refCount = 0;
      }
    } else {
      if (refCount == 20) {
	ref = -ref;
	refCount = 0;
      }
    }
  } else {
    if (refCount <= 400) {  // ref*2
      ref -= 1;
    } else {
      ref += 1;
    }
    if (refCount == 800) { // ref*4
      refCount = 0;
    }
  }
}


int main()
{
  int i,j;
  
  //Port directions
  outp(0x80,PORTB); // LED off
  outp(0x80,DDRB);  // output on LED
  outp(0x08,PORTC); // pull up on overtemp signals
  outp(0xa0,DDRC);  // output on dir and brake
  outp(0x80,PORTD); // pull up on reset switch
  outp(0x10,DDRD);  // output on pwm for motor 1
  
  outp(0x10,DDRA);  // test pin output

  /* Timer section */
176
177
  // Enable timer0 overflow interrupts
  outp(BV(TOIE0),TIMSK);
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
  
  /* Timer 1, 8 bit fast PWM no prescaling -> h_pwm=17.35 micros */
  outp(BV(COM1A1)|BV(COM1B1)|BV(WGM10),TCCR1A);
  // Fast PWM
  // outp(BV(WGM12)|BV(CS10),TCCR1B);
  // Phase and frequency correct pwm
  outp(BV(CS10),TCCR1B);
  /* Reset Timer1*/
  
  //outp(0x00,TCNT1H);   		// Reset TCNT1 high
  //outp(0x00,TCNT1L);		// Reset TCNT1 low

  
  
  /* Timer 2 (control loop), prescaler 256, clear on compare match (28), -> h = 0.5 ms */
193
194
  //  outp(BV(WGM21)|BV(CS22)|BV(CS21),TCCR2);
  //outp(28,OCR2);
195
  /* Reset timer 2 */
196
  //outp(0,TCNT2);
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211

  /* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */
  outp(BV(CS02)|BV(CS00),TCCR0);
  
  //Serial communication
  outp(0x00, UCSRA);	// USART:
  outp(0x98, UCSRB);	// USART: RxIntEnable|RxEnable|TxEnable
  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, MUX3?? for pendulum angle */
  outp(BV(REFS0)|BV(MUX0),ADMUX); 	
  
212
213
  // Enable ADC interrupts, start first conversion (what do adps0-2 do), prescaler 32
  outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229

  // outp(BV(ADEN)|BV(ADSC),ADCSRA);
  
  /* Wait a little bit, probably not needed...*/
  int tmp;
  for(i=0;i<2000;i++) 
    for(j=0;j<400;j++)
      tmp = j*j*j;
  

  //Enable interrupts
  sei();

  // loop
  while (1) {}
}