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
829752b6
Commit
829752b6
authored
Feb 19, 2009
by
Pontus Giselsson
Browse files
Almost final commit
parent
d941627b
Changes
3
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/current_control.c
deleted
100644 → 0
View file @
d941627b
/*********************************************
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
<avr/io.h>
#include
<avr/signal.h>
#include
<avr/interrupt.h>
#include
<inttypes.h>
// reference generation variables
volatile
int16_t
ref
=
100
;
volatile
int16_t
refCount
=
0
;
volatile
int8_t
refFlag
=
0
;
// control variables
volatile
unsigned
char
lbyte
,
hbyte
;
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
int16_t
I
=
0
;
volatile
int16_t
u
=
0
;
volatile
int16_t
K
=
74
;
//5;//375; // 7 frac bits
volatile
int16_t
Ke
=
26
;
//2;//6; //7 frac bits, K*h/Ti
volatile
int16_t
Ksat
=
44
;
//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
=
1000
;
volatile
int16_t
countSamples
=
0
;
volatile
int16_t
jj
=
0
;
volatile
int8_t
stop
=
0
;
volatile
int16_t
temp
;
// pwm sync variables
volatile
int8_t
pwmCycles
=
0
;
/* 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
){}
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
);
}
}
/* 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
++
;
}
}
/* Timer 1 compare match, start AD-conversion
* Additional changes: Dont use free running AD-conversion
* Enable overflow interrupts timer1
*/
SIGNAL
(
SIG_OUTPUT_COMPARE2
)
{
// Start AD conversion
ADCSRA
|=
BV
(
ADSC
);
// Code previously in sig_adc
PORTA
|=
0x10
;
//Turn on calculation timer
// Read input
low
=
inp
(
ADCL
);
high
=
inp
(
ADCH
);
y
=
((
high
<<
8
)
|
low
)
-
512
;
//y 9 frac bits
//control, since negative measurements
e
=
ref
+
y
;
//e 9 frac bits
//v = (int16_t)((K*e+I+64)>>7);
v
=
(
int16_t
)(((
K
*
e
+
64
)
>>
7
)
+
(
temp
>>
4
));
//saturation and update integral part of ctrl
if
(
v
>
511
)
{
temp
=
temp
+
(((
Ke
*
e
)
+
(
Ksat
)
*
(
511
-
v
))
>>
3
);
}
else
if
(
v
<
-
512
)
{
temp
=
temp
+
(((
Ke
*
e
)
+
(
Ksat
)
*
(
-
512
-
v
))
>>
3
);
}
else
{
temp
=
temp
+
((
Ke
*
e
)
>>
3
);
}
/*
//saturation and update integral part of ctrl
if (v > 511) {
I = I +(int32_t)((Ke*e) + (Ksat)*(511-v));
} else if (v < -512) {
I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
} else {
I = I +(int32_t)(Ke*e);
}
*/
// write output, inverting mode means set pwm to 127-ctrl_out
// Original styrning med 7 bitar + direction
u
=
(
v
+
2
)
>>
2
;
//7 frac bits to pwm
if
(
u
>
127
)
{
u
=
127
;
}
else
if
(
u
<
-
128
)
{
u
=
-
128
;
}
if
(
u
<
0
)
{
PORTC
|=
0x80
;
OCR1BL
=
(
unsigned
char
)
(
128
-
(
-
u
));
}
else
{
PORTC
=
(
PORTC
&
0x7F
);
OCR1BL
=
(
unsigned
char
)
(
127
-
u
);
}
// For logging
countSamples
++
;
if
(
countSamples
==
skipSamples
)
{
ctrl_log
[
jj
]
=
y
;
I_log
[
jj
]
=
u
;
error_log
[
jj
]
=
e
;
jj
++
;
countSamples
=
0
;
// Stop after a while
if
((
jj
==
(
log_len
-
1
))
&
!
stop
)
{
outp
(
0x7f
,
OCR1BL
);
stop
=
1
;
sendData
();
}
}
PORTA
&=
0xef
;
//Turn off calculation timer
}
/* 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
==
20
)
{
refFlag
=
1
;
ref
=
-
ref
;
refCount
=
0
;
}
}
else
{
if
(
refCount
==
40
)
{
ref
=
-
ref
;
refCount
=
0
;
}
}
}
else
{
if
(
refCount
<=
200
)
{
// ref
ref
-=
1
;
}
else
{
ref
+=
1
;
}
if
(
refCount
==
400
)
{
// ref*2
refCount
=
0
;
}
}
}
int
main
()
{
cli
();
int
i
,
j
;
//Port directions
outp
(
0x08
,
PORTC
);
// pull up on overtemp signals
outp
(
0xa0
,
DDRC
);
// output on dir and brake
outp
(
0x10
,
DDRD
);
// output on pwm for motor 1
outp
(
0x10
,
DDRA
);
// test pin output
/* Timer section */
// Enable timer0 overflow interrupts
// and enable timer1 overflow interrupts for pwm-control sync
outp
(
BV
(
TOIE0
)
|
BV
(
OCIE2
),
TIMSK
);
// 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)
// 7 bit ctrl-signal and direction
outp
(
0x00
,
OCR1AH
);
outp
(
0x7f
,
OCR1AL
);
outp
(
0x00
,
TCNT1H
);
outp
(
0x00
,
TCNT1L
);
// 7 bit ctrl-signal and direction
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
);
/* 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, prescaler 32, free running mode
//outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC interrupts, start first conversion, prescaler 32, not free running mode
//outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC, start first conversion, prescaler 32, not free running mode
outp
(
BV
(
ADEN
)
|
BV
(
ADSC
)
|
BV
(
ADPS2
)
|
BV
(
ADPS0
),
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
)
{
if
(
stop
)
{
cli
();
OCR1BL
=
0xff
;
}
}
}
linear_pendulum_2009/avr/current_control_final.c
View file @
829752b6
/***************************************************************
Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
for LTH - reglerteknik
/***************************************************************
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
...
...
@@ -6,27 +12,24 @@
// control variables
volatile
int16_t
ref
=
0
;
// reference value (from velocity controller)
volatile
int16_t
y
;
// measurement, 9 frac bits
volatile
uint16_t
low
,
high
;
// when reading AD-conversion
volatile
int16_t
e
=
0
;
// control error, 9 frac bits
volatile
int16_t
v
=
0
;
// temporary ctrl signal, 9 frac bits
volatile
int16_t
vSat
=
0
;
volatile
int16_t
I
=
0
;
// integral part of ctrl, 13 frac bits
volatile
int16_t
u
=
0
;
// ctrl signal = pwm high time (8 bits)
volatile
int16_t
K
=
111
;
// 7 frac bits
volatile
int16_t
Ke
=
38
;
// 7 frac bits, K*h/Ti
volatile
int8_t
intCond
=
0
;
#define V_MAX 508
volatile int16_t ref=0; // reference value (from velocity controller)
volatile int16_t y; // measurement, 9 frac bits
volatile uint16_t low, high; // when reading AD-conversion
volatile int16_t e = 0; // control error, 9 frac bits
volatile int16_t v = 0; // temporary ctrl signal, 9 frac bits
volatile int16_t vSat = 0; // saturated 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 (8 bits)
volatile int16_t K = 111; // 7 frac bits
volatile int16_t Ke = 38; // 7 frac bits, K*h/Ti
volatile int8_t intCond = 0; // flag for conditional integration
#define V_MAX 508 //max/min for saturation
#define V_MIN -508
// twi variable
volatile int16_t status;
// logging variables
/*
#define log_len 100
...
...
@@ -39,9 +42,6 @@ volatile int16_t jj=0;
volatile int8_t stop = 0;
*/
/* Routine used to transmit bytes on the serial port */
/*
static void putchar(unsigned char ch)
...
...
@@ -80,7 +80,6 @@ static inline void sendData() {
/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL
(
SIG_OUTPUT_COMPARE2
)
{
PORTB
|=
0x40
;
// Start AD conversion
ADCSRA
|=
BV
(
ADSC
);
...
...
@@ -159,7 +158,6 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
}
*/
PORTB
&=
~
0x40
;
}
...
...
@@ -175,7 +173,7 @@ int main()
outp
(
0x40
,
DDRB
);
// temp pwm output
/* Timer section */
// Timer 1, fast PWM no prescaling (non-inverting mode (start
low
, switch to
high
))
// Timer 1, fast PWM no prescaling (non-inverting mode (start
high
, switch to
low
))
outp
(
BV
(
COM1B1
)
|
BV
(
WGM11
)
|
BV
(
WGM10
),
TCCR1A
);
outp
(
BV
(
CS10
)
|
BV
(
WGM13
)
|
BV
(
WGM12
),
TCCR1B
);
...
...
linear_pendulum_2009/avr/vel_control.c
View file @
829752b6
...
...
@@ -24,8 +24,8 @@ volatile int32_t e = 0; // 11 frac bits
volatile
int8_t
intCond
=
0
;
volatile
int32_t
K
=
80
0
;
// 6 frac bits, prop constant
volatile
int32_t
Ke
=
3
0
;
// 6 frac bits, integral constant
volatile
int32_t
K
=
80
7
;
// 6 frac bits, prop constant
volatile
int32_t
Ke
=
1
3
;
// 6 frac bits, integral constant
volatile
int8_t
fr_comp
=
(
10
<<
3
);
#define V_MAX (120<<4)
#define V_MIN (-120<<4)
...
...
@@ -56,7 +56,6 @@ volatile int16_t low; // temporary variable for ad-reading
volatile
int16_t
high
;
// temporary variable for ad-reading
volatile
int16_t
angleOffset
=
0
;
/* return position (in tics) */
int32_t
getPosition
()
{
cli
();
...
...
@@ -136,6 +135,8 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
TIMSK
&=
~
(
BV
(
OCIE0
)
|
BV
(
OCIE1A
));
// Disable communication and ctrl-interrupts
sei
();
// enable interrupts from encoder-counter
// Poll UART receiver
uint8_t
status
=
UCSRA
;
...
...
@@ -153,7 +154,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
int16_t
toSend
=
pccom_getNextByteToSend
();
if
(
toSend
>=
0
)
UDR
=
(
char
)
toSend
;
}
TIFR
=
(
1
<<
OCF0
);
// skip pending interrupts from serial comm, (but not from ctrl)
TIMSK
|=
(
BV
(
OCIE0
)
|
BV
(
OCIE1A
));
// reenable communication and ctrl-interrupts
...
...
@@ -221,11 +224,16 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
uSend
=
u
;
sei
();
// TWI-communication to set current reference on the other atmel
// 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
);
...
...
@@ -235,8 +243,7 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
outp
((
int8_t
)
u
,
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
);
...
...
@@ -272,10 +279,11 @@ int main()
outp
(
0
,
TCNT1L
);
/* Timer 0,
1
0 kHz, Prescaler 8, serial communication */
/* Timer 0,
4
0 kHz, Prescaler 8, serial communication */
outp
(
BV
(
WGM01
)
|
BV
(
CS01
),
TCCR0
);
outp
(
184
-
1
,
OCR0
);
// 10 kHz
/* Reset timer 0 */
//outp(184-1,OCR0); // 10 kHz
outp
(
46
-
1
,
OCR0
);
// 40 kHz
/* Reset timer 0 */
outp
(
0
,
TCNT0
);
...
...
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