Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Anders Blomdell
processer
Commits
2adcd4c2
Commit
2adcd4c2
authored
Jan 22, 2009
by
Pontus Giselsson
Browse files
PI-current-control, with logging, 0.56 s runtime, measurement y-route 1 nF filter'
parent
ca14d409
Changes
1
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/current_control.c
View file @
2adcd4c2
...
...
@@ -17,7 +17,7 @@
// reference generation variables
volatile
int16_t
ref
=
2
00
;
volatile
int16_t
ref
=
1
00
;
volatile
int16_t
refCount
=
0
;
volatile
int8_t
refFlag
=
0
;
...
...
@@ -25,14 +25,30 @@ volatile int8_t refFlag = 0;
// control variables
volatile
unsigned
char
lbyte
,
hbyte
;
volatile
int16_t
result
;
volatile
int16_t
ctrl
;
volatile
int16_t
ctrl_out
;
volatile
int16_t
temp
;
volatile
int16_t
y
;
volatile
uint8_t
alt
=
1
;
volatile
uint8_t
low
,
high
;
//test variables
volatile
int16_t
e
=
0
;
volatile
int16_t
v
=
0
;
volatile
int32_t
I
=
0
;
volatile
int16_t
u
=
0
;
volatile
int16_t
K
=
20
;
//5;//375; // 7 frac bits
volatile
int16_t
Ke
=
8
;
//2;//6; //7 frac bits, K*h/Ti
volatile
int16_t
Ksat
=
51
;
//1; // 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
=
200
;
volatile
int16_t
countSamples
=
0
;
volatile
int16_t
jj
=
0
;
volatile
int8_t
stop
=
0
;
volatile
int16_t
temp
;
/* Routine used to set the red LED */
...
...
@@ -86,48 +102,97 @@ static inline void writeOutput(int16_t val) {
}
/* Send logged data over Serial connection */
static
inline
void
sendData
()
{
int16_t
ii
=
0
;
while
(
ii
<
log_len
)
{
putchar
((
unsigned
char
)
((
ctrl_log
[
ii
]
&
0xff00
)
>>
8
));
putchar
((
unsigned
char
)
(
ctrl_log
[
ii
]
&
0x00ff
));
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
++
;
}
}
/* Interrupt when AD-conversion completes */
SIGNAL
(
SIG_ADC
)
{
setPA4
(
1
);
// Read input
low
=
inp
(
ADCL
);
high
=
inp
(
ADCH
);
result
=
((
high
<<
8
)
|
low
)
-
512
;
y
=
((
high
<<
8
)
|
low
)
-
512
;
//y 9 frac bits
// Scale incoming current measurement
result
=
result
*
3
;
//!!!!!!!!!!!!!!!!!!!!!!!
//y = y
*3; //!!!!!!!!!!!!!!!!!!!!!!!
// Scale incoming current measurement
// result
+=
13
;
//control
ctrl
=
ref
-
result
;
ctrl
=
((
ctrl
+
(
1
<<
2
))
>>
3
);
y
+=
70
;
//control
, since negative measurements
e
=
ref
+
y
;
//e 9 frac bits
v
=
(
int16_t
)(((
K
*
e
+
(
1
<<
6
))
>>
7
)
+
(
int16_t
)((
I
+
(
1
<<
6
))
>>
7
));
//saturation/rounding to 8 bit
if
(
ctrl
>
511
)
ctrl_out
=
511
;
else
if
(
ctrl
<
-
51
1
)
ctrl_out
=
-
51
1
;
if
(
v
>
511
)
u
=
511
;
else
if
(
v
<
-
51
2
)
u
=
-
51
2
;
else
ctrl_out
=
ctrl
;
u
=
v
;
I
=
I
+
((
int32_t
)
Ke
)
*
((
int32_t
)
e
)
+
((
int32_t
)
Ksat
)
*
((
int32_t
)(
u
-
v
));
//16 frac bits
// write output, inverting mode means set pwm to 127-ctrl_out
u
=
(
u
)
>>
2
;
//7 frac bits to pwm
// write output
if
(
ctrl_out
<
0
)
{
if
(
u
<
0
)
{
PORTC
=
0x80
+
(
PORTC
&
0x7F
);
OCR1BL
=
(
unsigned
char
)
(
-
ctrl_out
);
OCR1BL
=
(
unsigned
char
)
(
128
-
(
-
u
)
);
}
else
{
PORTC
=
(
PORTC
&
0x7F
);
OCR1BL
=
(
unsigned
char
)
(
ctrl_out
);
OCR1BL
=
(
unsigned
char
)
(
127
-
u
);
}
// For logging
countSamples
++
;
if
(
countSamples
==
skipSamples
)
{
ctrl_log
[
jj
]
=
y
;
I_log
[
jj
]
=
I
;
error_log
[
jj
]
=
e
;
jj
++
;
countSamples
=
0
;
}
if
((
jj
==
(
log_len
-
1
))
&
!
stop
)
{
outp
(
0x7f
,
OCR1BL
);
stop
=
1
;
sendData
();
}
setPA4
(
0
);
}
/* reference square- or triangle wave generator with timer 0 */
SIGNAL
(
SIG_OVERFLOW0
)
{
int8_t
rectangle
=
1
;
...
...
@@ -146,12 +211,12 @@ SIGNAL(SIG_OVERFLOW0) {
}
}
}
else
{
if
(
refCount
<=
40
0
)
{
// ref*2
ref
-=
1
;
if
(
refCount
<=
2
0
)
{
// ref*2
ref
-=
2
;
}
else
{
ref
+=
1
;
ref
+=
2
;
}
if
(
refCount
==
80
0
)
{
// ref*4
if
(
refCount
==
4
0
)
{
// ref*4
refCount
=
0
;
}
}
...
...
@@ -160,6 +225,7 @@ SIGNAL(SIG_OVERFLOW0) {
int
main
()
{
cli
();
int
i
,
j
;
//Port directions
...
...
@@ -176,16 +242,17 @@ int main()
// Enable timer0 overflow interrupts
outp
(
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*/
// 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
);
//outp(0x00,TCNT1H); // Reset TCNT1 high
//outp(0x00,TCNT1L); // Reset TCNT1 low
// 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
...
...
@@ -216,14 +283,19 @@ int main()
/* 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
;
//
for(i=0;i<2000;i++)
//
for(j=0;j<400;j++)
//
tmp = j*j*j;
//Enable interrupts
sei
();
// loop
while
(
1
)
{}
while
(
1
)
{
if
(
stop
)
{
cli
();
OCR1BL
=
0x7f
;
}
}
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment