current_control.c 5.81 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
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
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
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
/*********************************************

 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>

volatile int16_t ref = 20;
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
volatile uint8_t alt = 1;


/* 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){}
/* Read 10-bit input using the AD converter */
static inline int16_t readInput() {
  uint8_t low, high;
  ADCSRA |= 0x40;
  while (ADCSRA & 0x40);
  low = ADCL;
  high = ADCH;
  return ((high<<8) | low) - 512;
}


/* 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);
  }

  //unsigned char lbyte,hbyte;
  //hbyte = ((val & 0xff00)>>8);	//Get ADC result high byte
  //lbyte = ((val & 0x00ff));	//Get ADC result low byte

  //putchar(hbyte);
  //putchar(lbyte);
}


/* Periodic timer interrupt */
SIGNAL(SIG_OUTPUT_COMPARE2)
{
  

  // PORTA &= 0xef;
  unsigned char lbyte,hbyte;  
  int16_t result;
  int16_t ctrl; 
  int16_t ctrl_out;
  int16_t temp;
  
  setPA4(alt);

  result = readInput();

  setPA4(0);  

  result = result*3; //!!!!!!!!!!!!!!!!!!!!!!!

  // Compensate for offset in measurements
  // result += 13;

  //hbyte = (((ref-result) & 0xff00)>>8);	//Get ADC result high byte
  //lbyte = (((ref-result) & 0x00ff));	        //Get ADC result low byte
  
  // Transmit data
  //putchar(hbyte);	//high byte
  //putchar(lbyte);	//low byte

  //hbyte = ((result & 0xff00)>>8);	//Get ADC result high byte
  //lbyte = ((result & 0x00ff));	//Get ADC result low byte
  
  // Transmit data
  //putchar(hbyte);	//high byte
  //putchar(lbyte);	//low byte
  
  // control
  ctrl = ref-result;
  ctrl = ((ctrl + (1<< 2)) >> 3);
  
  //saturation/rounding to 8 bit
  if (ctrl > 127)
    ctrl_out = 127;
  else if (ctrl < -128)
    ctrl_out = -128;
  else
    ctrl_out = ctrl;
 
  //writeOutput(ctrl_out);
  writeOutput(ref);


 
  temp = (int16_t) ctrl_out;
  hbyte = ((temp & 0xff00)>>8);	//Get ADC result high byte
  lbyte = ((temp & 0x00ff));	//Get ADC result low byte
  putchar(hbyte); 
  putchar(lbyte);
 
}


/* 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 */
  // Enable TCNT2 (timer2) compare match interrupts, and timer0 overflow interrupts
  outp(BV(OCIE2)|BV(TOIE0),TIMSK);
  
  /* 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 */
  outp(BV(WGM21)|BV(CS22)|BV(CS21),TCCR2);
  outp(28,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);

  /* 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); 	
  
  // Enable ADC interrupts, start first conversion (what do adps0-2 do), prescaler 128
  // outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSRA);
  // Enable ADC interrupts, start first conversion (what do adps0-2 do), prescaler 8
  // outp(BV(ADEN)|BV(ADSC)|BV(ADPS1)|BV(ADPS0),ADCSRA);

  // Enable ADC interrupts, start first conversion (what do adps0-2 do), prescaler 64
  outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS1),ADCSRA);

  // 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) {}
}