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
9021297a
Commit
9021297a
authored
Jan 09, 2009
by
Pontus Giselsson
Browse files
Added folders for Embedded Control course
parent
253ca6ca
Changes
2
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/Makefile
0 → 100644
View file @
9021297a
PROJECT
=
current_control
CHIP
=
atmega16
ARCH
=
avr
AVRSOURCE
=
AVRAUTO
=
compiled/
$(ARCH)
/
AVRARCH
=
compiled/
$(ARCH)
/
LIB_TARGET
=
$(AVRARCH)
lib
$(TARGET)
.a
CC
=
$(ARCH)
-gcc
CXX
=
$(ARCH)
-gcc
CCFLAGS
=
-mmcu
=
$(CHIP)
-O5
-I
.
-I
$(AVRAUTO)
AS
=
$(ARCH)
-as
ASFLAGS
=
-mmcu
=
$(CHIP)
AR
=
$(ARCH)
-ar
LD
=
$(ARCH)
-ld
OBJDUMP
=
$(ARCH)
-objdump
LDFLAGS
=
-T
$(CHIP)
.link
all
:
$(AVRARCH)$(PROJECT)
$(LIB_TARGET)(%.o)
:
$(AVRSOURCE)%.c Makefile
$(CC)
$(CCFLAGS)
-c
-o
$(AVRARCH)
$
(
%
)
-I
$(AVRSOURCE)
-I
$(AVRAUTO)
$
(
<
)
$(AR)
r
$
(
@
)
$(AVRARCH)
$
(
%
)
rm
$(AVRARCH)
$
(
%
)
$(LIB_TARGET)(%.o)
:
$(AVRSOURCE)%.cc Makefile
$(CXX)
$(CCFLAGS)
-c
-o
$(AVRARCH)
$
(
%
)
-I
$(AVRSOURCE)
-I
$(AVRAUTO)
$
(
<
)
$(AR)
r
$
(
@
)
$(AVRARCH)
$
(
%
)
rm
$(AVRARCH)
$
(
%
)
$(LIB_TARGET)(%.o)
:
$(AVRSOURCE)%.s Makefile
$(AS)
$(ASFLAGS)
-o
$(AVRARCH)
$
(
%
)
$(AVRSOURCE)
$
(
*
)
.s
$(AR)
r
$
(
@
)
$(AVRARCH)
$
(
%
)
rm
$(AVRARCH)
$
(
%
)
$(AVRARCH)%.o
:
$(AVRSOURCE)%.s Makefile
$(AS)
$(ASFLAGS)
-o
$@
$(AVRSOURCE)
$
(
*
)
.s
$(AVRARCH)%.o
:
$(AVRSOURCE)%.c Makefile
$(CC)
$(CCFLAGS)
-c
-o
$@
-I
.
-I
$(AVRAUTO)
$<
$(AVRARCH)%
:
$(AVRARCH)%.o Makefile
$(CC)
$(CCFLAGS)
-o
$@
$<
-lm
$(AVRARCH)%.sr
:
all $(AVRARCH)%
avr-objcopy
-O
srec
$(AVRARCH)
/
$
(
*
)
$@
%.sr
:
$(AVRARCH)%.sr
@
/bin/true
%.load
:
compiled/avr/%.sr
# /work/andersb/atmel/uisp/uisp-0.2b/src/uisp \
# -dno-poll -dstk200 --erase --upload if=$(AVRARCH)/$(*).sr
uisp
\
-dprog
=
stk200
\
--erase
\
--upload
if
=
compiled/avr/
$*
.sr
%.load.stk500
:
compiled/avr/%.sr
uisp
\
-dprog
=
stk500
\
--erase
\
--upload
if
=
compiled/avr/
$*
.sr
%.dump
:
all $(AVRARCH)/%
$(OBJDUMP)
-D
$(AVRARCH)
/
$
(
*
)
$(AVRARCH)fuse.sr
:
fuse.s
avr-gcc
-o
$(AVRARCH)
fuse.o
-c
fuse.s
avr-objcopy
-O
srec
$(AVRARCH)
fuse.o
$(AVRARCH)
fuse.sr
fuse.load
:
$(AVRARCH)fuse.sr
uisp
\
-dprog
=
stk200
\
--erase
\
--segment
=
fuse
\
--upload
if
=
compiled/avr/fuse.sr
load.stk500
:
$(PROJECT).load.stk500
\ No newline at end of file
linear_pendulum_2009/avr/current_control.c
0 → 100644
View file @
9021297a
/*********************************************
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>
//Main control parameters
#define MODE 3 //1 = velocity, 2 = current, 3 = cascade, 0 = control off
#define U_MAX 400 //Voltage saturation point
#define U_FIXEDPOINT 3000 //Wanted current, used when MODE = 2, note this is in fixed point arithmetic 2^6
#define U_REAL 100 //When MODE = 1, voltage used for left-right motion
#define NBR_CHAN 1
//Current regulation parameters
#define KC_P 2 //Proportional constant
#define KC_I 0 //Integral constant (currently 0 as PI_C is run as many times as PI_V integral control
// -does not make sense when in cascade) for MODE = 2, KC_I = 5 is ok
#define ANTI_WINDUP 10 //Anti windup constant
//Current regulation variables
volatile
long
v_C
=
0
;
//Voltage
volatile
long
e_C
=
0
;
//Error
volatile
long
I_C
=
0
;
//Integral part
volatile
long
a_C
=
0
;
//Current (amperes)
volatile
long
u_C
=
0
;
//Used for anti windup
volatile
long
U
=
0
;
//= +/- U_FIXEDPOINT, used for current regulation, MODE = 2
//Velocity regulation parameters
#define V_REQ 70 //Required velocity, note the magnitude of this depends on STEP
#define KV_P 8 //Proportional constant
#define KV_I 2 //Integral constant
//Velocity regulation variables
volatile
long
v_V
;
//Voltage
volatile
long
e_V
=
5
;
//Error
volatile
long
I_V
=
0
;
//Integral part
volatile
long
u_V
=
0
;
volatile
long
VEL
=
0
;
//= +/- V_REQ
//Position/velocity parameters
#define STEP 100 //Amount of steps between position measurements, used for determining velocity
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
//Position/velocity variables
volatile
int
positions
[
STEP
];
//Array holding history of position at each sample
volatile
int
currentPos
;
//Current sample
volatile
long
vel
=
0
;
//Current velocity
volatile
int
oldX
;
//Used for positional encoders
volatile
int
oldY
;
//Used for positional encoders
volatile
int
sum
;
//Used for positional encoders
volatile
long
pos
=
0
;
//Current position
//Sampling variables
volatile
int
sendData
[
8
]
=
{
0
,
7
,
6
,
5
,
4
,
3
,
2
,
1
};
//Array holding data that is sent through serial port
volatile
char
dataAvailable
=
0
;
//Flag if an unhandled ADC has been completed
volatile
int
nbrSampSinceLastSend
=
0
;
//Checks if a sample has been missed and if so discards it
volatile
char
logStarted
=
0
;
//Flag
volatile
int
dir
=
0
;
//Direction of motion (+ = right, - = left)
/* Routine used to set the red LED */
void
setLED
(
uint8_t
on
)
{
if
(
on
)
PORTB
&=
~
0x80
;
//Turn on
else
PORTB
|=
0x80
;
//Turn off
}
/* Routine used to initialize the positional encoders */
void
initPos
()
{
oldX
=
ENCODERX
;
oldY
=
ENCODERY
;
}
/* Routine used to track the cart position */
void
setPos
()
{
int
newX
=
ENCODERX
;
int
newY
=
ENCODERY
;
if
((
newX
!=
oldX
)
||
(
newY
!=
oldY
))
//Check if any value changed
{
sum
=
(
oldX
<<
2
)
+
oldY
+
newX
+
(
newY
>>
2
);
//Find state
if
(
sum
==
2
||
sum
==
4
||
sum
==
11
||
sum
==
13
)
//Predetermined values determine direction
pos
++
;
else
pos
--
;
oldX
=
newX
;
oldY
=
newY
;
}
}
/* Routine used to set the PWM duty cycle */
void
setMotorVoltage
(
int
axis
,
int
u
)
{
int
pwm
;
u
>>=
1
;
//Taking specified 10bit value but PWM is 9bit, therefore compensate
if
(
axis
==
0
)
//Axis determines which motor is used but currently only one motor available
{
if
(
u
<
0
)
{
//Determine direction
pwm
=
-
u
;
PORTC
|=
0x80
;
}
else
{
pwm
=
u
;
PORTC
&=
~
0x80
;
}
if
(
pwm
>
0x3ff
)
{
pwm
=
0x3ff
;
}
OCR1B
=
pwm
&
0x3ff
;
//Set PWM duty cycle
}
}
/* 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
)
{};
}
/* Routine for updating the motor voltage input with current control */
void
PI_C
(
long
input
,
long
ref
)
{
//Convert adc input to current. Calculations listed in trac-wiki
a_C
=
input
*
5
-
2601
;
//Get current
a_C
*=
3
;
//Convert it to
e_C
=
ref
+
a_C
;
v_C
=
KC_P
*
(
e_C
)
+
KC_I
*
I_C
+
60
*
dir
;
//Set voltage
v_C
=
(
v_C
>>
6
);
//Remove fixed point arithmetic
if
(
v_C
<=
U_MAX
&&
v_C
>-
U_MAX
)
//Check for saturation
{
u_C
=
v_C
;
setLED
(
1
);
//Notify that PI is running
}
else
{
if
(
v_C
<
0
)
u_C
=-
U_MAX
;
else
u_C
=
U_MAX
;
setLED
(
0
);
}
if
((
e_C
>>
5
)
==
0
)
//Depending on sign 0 gets rounded as 0 or -1, compensate
I_C
+=
1
+
ANTI_WINDUP
*
(
u_C
-
v_C
);
else
I_C
+=
(
e_C
>>
5
)
+
ANTI_WINDUP
*
(
u_C
-
v_C
);
//Update integral part
setMotorVoltage
(
0
,
v_C
);
//Set voltage
}
/* Routine for updating the input for the current control with speed regulation */
void
PI_V
(
long
input
)
{
e_V
=
(
VEL
<<
4
)
-
(
input
<<
4
);
//Calculate error
v_V
=
KV_P
*
e_V
+
KV_I
*
I_V
;
//Set voltage
v_V
=
(
v_V
>>
4
)
+
60
*
dir
;
//Remove fixed point arithmetic and add friction compensation
if
(
v_V
<=
U_MAX
&&
v_V
>-
U_MAX
)
//Check for saturation
{
u_V
=
v_V
;
setLED
(
1
);
//Notify that PI is running
}
else
{
if
(
v_V
<
0
)
u_V
=-
U_MAX
;
else
u_V
=
U_MAX
;
setLED
(
0
);
//Notify saturation
}
if
((
e_V
>>
6
)
==
0
)
//Again, sign rounding compensation
I_V
++
;
else
I_V
+=
e_V
>>
6
;
//Update integral part
if
(
MODE
==
1
)
setMotorVoltage
(
0
,
u_V
);
//Set voltage if in velocity control, otherwise cascaded current will set voltage
}
/* Interrupt service routing for handling timer 1 */
SIGNAL
(
SIG_OVERFLOW1
)
{
static
long
ctr1
;
static
long
ctr2
;
ctr1
++
;
//Sinewave motion in steps
if
(
ctr1
>=
3
)
{
outp
(
BV
(
ADEN
)
|
BV
(
ADSC
)
|
BV
(
ADIE
)
|
BV
(
ADPS2
)
|
BV
(
ADPS1
)
|
BV
(
ADPS0
),
ADCSR
);
//Request ADC
ctr2
++
;
if
(
ctr2
>=
8000
)
//Delay between direction switch
{
//Right-left motion
if
(
dir
==-
1
)
{
if
(
MODE
)
//Check that control is on
{
U
=
-
U_FIXEDPOINT
;
//Set required current (in fixed point arithmetic)
VEL
=
-
V_REQ
;
//Set required velocity
I_C
=
0
;
//Reset integral part
I_V
=
0
;
//Reset integral part
}
else
{
setMotorVoltage
(
0
,
-
U_REAL
);
//If no control simply set voltage to required value
}
dir
=
1
;
//Change direction
}
else
{
if
(
MODE
)
//Repeat of previous if statement but in reverse direction
{
U
=
U_FIXEDPOINT
;
VEL
=
V_REQ
;
I_C
=
0
;
I_V
=
0
;
}
else
{
setMotorVoltage
(
0
,
U_REAL
);
}
dir
=
-
1
;
}
ctr2
=
0
;
//Reset counter
}
ctr1
=
0
;
//Reset counter
}
}
/* Not currently in use but necessary to catch interrupt on timer0 overflow */
SIGNAL
(
SIG_OVERFLOW0
)
{
static
int
ctr0
;
ctr0
++
;
if
(
ctr0
>=
10
)
{
//putchar('O');
ctr0
=
0
;
}
}
/* Interrupt service routine for handling ADC interrupts */
SIGNAL
(
SIG_ADC
)
{
static
int
ctrA
;
unsigned
char
lbyte
,
hbyte
;
ctrA
++
;
lbyte
=
inp
(
ADCL
);
//Get ADC result low byte
hbyte
=
inp
(
ADCH
);
//Get ADC result high byte
if
(
ctrA
>=
1
)
{
if
(
logStarted
==
1
)
{
cli
();
dataAvailable
=
1
;
//Flag data being available
nbrSampSinceLastSend
++
;
//Increase samples since last send
sendData
[
0
]
=
(
int
)((
hbyte
<<
8
)
+
lbyte
);
//Store result in data array
sei
();
}
else
{
dataAvailable
=
1
;
sendData
[
0
]
=
(
int
)((
hbyte
<<
8
)
+
lbyte
);
}
ctrA
=
0
;
}
}
/* Interrupt service routine for handling incoming bytes on the serial port */
SIGNAL
(
SIG_UART_RECV
)
{
char
ch
=
inp
(
UDR
);
//Read input
cli
();
logStarted
=
1
;
//Flag log started
sei
();
}
/* Interrupt service routine for timer 0; Not in use */
/*SIGNAL(SIG_OVERFLOW0)
{
static int ctr0;
ctr0++;
if (ctr0>=10)
{
//putchar('O');
ctr0 = 0;
}
}*/
int
main
()
{
int
i
,
j
;
//Used for for loops
//Port directions
outp
(
0x80
,
PORTB
);
// LED off
outp
(
0x80
,
DDRB
);
// output on LED
outp
(
0x0c
,
PORTC
);
// pull up on overtemp signals
outp
(
0xf0
,
DDRC
);
// output on dir and brake
outp
(
0x80
,
PORTD
);
// pull up on reset switch
outp
(
0x30
,
DDRD
);
// output on pwm1&2
//Counters
outp
(
BV
(
TOIE0
)
|
BV
(
TOIE1
),
TIMSK
);
// Enable TCNT0 and TCNT1 overflow interrupts
outp
(
0x00
,
TCNT0
);
// Reset TCNT0, CLK/1024
outp
(
0x00
,
TCNT1H
);
// Reset TCNT1 high, CLK/512
outp
(
0x00
,
TCNT1L
);
// Reset TCNT1 low, CLK/512
outp
(
BV
(
CS02
)
|
BV
(
CS00
),
TCCR0
);
// 1024
//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
// Timer 1 (PWM) 14.7456MHz/1/512 -> 28.8kHz
// PWM needs to run at 1/512 in order to avoid vibration excitation
// OC1A & OC1B 9 bit fast PWM, active high
outp
(
BV
(
COM1A1
)
|
BV
(
COM1B1
)
|
BV
(
WGM11
),
TCCR1A
);
outp
(
BV
(
WGM12
)
|
BV
(
CS10
),
TCCR1B
);
// No prescaler, or /1...
outp
(
BV
(
REFS0
)
|
BV
(
MUX0
),
ADMUX
);
//AREF (AREF is 5V) pin external capacitor, MUX0 for current, MUX3 for pendulum angle
// Enable ADC interrupts, CLK/128
outp
(
BV
(
ADEN
)
|
BV
(
ADSC
)
|
BV
(
ADIE
)
|
BV
(
ADPS2
)
|
BV
(
ADPS1
)
|
BV
(
ADPS0
),
ADCSR
);
unsigned
int
data
[
8
]
=
{
1
,
2
,
3
,
4
,
5
,
6
,
7
,
8
};
//Reset data array
/* 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
;
//Initialize encoder
initPos
();
//Intialize velocity
for
(
j
=
0
;
j
<
STEP
;
j
++
)
positions
[
j
]
=
0
;
currentPos
=
STEP
;
sei
();
//Enable interrupts
for
(;;)
{
//Update positional encoder
setPos
();
if
(
dataAvailable
&&
!
logStarted
)
{
dataAvailable
=
0
;
//Reset flag
//Update velocity
vel
=
pos
-
positions
[(
currentPos
+
1
)
<
STEP
?
currentPos
+
1
:
0
];
//Determine velocity
positions
[
currentPos
]
=
pos
;
//Update position array
currentPos
++
;
//Update current sample
if
(
currentPos
>=
STEP
)
currentPos
-=
STEP
;
//If overflow, loop around
if
(
MODE
==
1
||
MODE
==
3
)
PI_V
(
vel
);
//Velocity or cascade control
if
(
MODE
==
3
)
PI_C
(
sendData
[
0
],
u_V
<<
6
);
//Cascade control, voltage is put in fixed point arithmetic
if
(
MODE
==
2
)
PI_C
(
sendData
[
0
],
U
);
//Velocity control
}
while
(
logStarted
==
1
&&
dataAvailable
==
1
)
{
cli
();
// Enter critical section: disable interrupts
for
(
i
=
0
;
i
<
NBR_CHAN
;
i
++
)
{
data
[
i
]
=
(
unsigned
int
)
sendData
[
i
];
//Store data locally
}
//Update velocity
vel
=
pos
-
positions
[(
currentPos
+
1
)
<
STEP
?
currentPos
+
1
:
0
];
//Determine velocity
positions
[
currentPos
]
=
pos
;
//Update position array
currentPos
++
;
//Update current sample
if
(
currentPos
>=
STEP
)
currentPos
-=
STEP
;
//If overflow, loop around
data
[
2
]
=
(
unsigned
int
)
vel
;
//Parameter that can be sent through SerialLogger
if
(
nbrSampSinceLastSend
>
1
)
data
[
0
]
=
0
;
//Check if sample was missed
dataAvailable
=
0
;
//Reset flag
nbrSampSinceLastSend
=
0
;
sei
();
// Exit critical section: enable interrupts
if
(
MODE
==
1
||
MODE
==
3
)
PI_V
(
vel
);
//Velocity or cascade control
if
(
MODE
==
3
)
PI_C
(
sendData
[
0
],
u_V
<<
6
);
//Cascade control, voltage is put in fixed point arithmetic
if
(
MODE
==
2
)
PI_C
(
sendData
[
0
],
U
);
//Velocity control
// Transmit data
for
(
i
=
0
;
i
<
NBR_CHAN
;
i
++
)
{
putchar
((
unsigned
char
)((
data
[
i
+
2
]
&
0xff00
)
>>
8
));
//Low byte
putchar
((
unsigned
char
)((
data
[
i
+
2
]
&
0x00ff
)));
//High byte
}
}
}
}
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