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
88702ed0
Commit
88702ed0
authored
May 11, 2015
by
Anders Blomdell
Browse files
Making vel and acceleration ref active at the same time.
parent
07e26162
Changes
1
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/vel_control.c
View file @
88702ed0
...
...
@@ -43,23 +43,30 @@
static
volatile
uint8_t
pccom_poll
=
0
;
// Keep alive timer
volatile
uint16_t
nbrSamples
=
0
;
// nbr of samples between ctrl-ref updates
// reference variables
volatile
int32_t
g_vel_ref
=
0
;
// 11 frac bits (variable in mm/s, max 2^12)
struct
{
volatile
uint16_t
samples
;
// nbr of samples since last ctrl-ref update
struct
{
volatile
unsigned
char
pending
;
volatile
int32_t
next
;
volatile
int32_t
value
;
}
vel
;
struct
{
volatile
int32_t
value
;
}
acc
;
}
ref
=
{
.
samples
=
0
,
.
vel
.
pending
=
0
,
.
vel
.
next
=
0
,
.
vel
.
value
=
0
,
.
acc
.
value
=
0
,
};
volatile
int32_t
refCtrl
=
0
;
// ref used in ctrl-loop (=ref sent from simulink)
volatile
int32_t
g_acc_ref
=
0
;
// 14 frac bits (variable in mm/s, max 2^15)
// velocity control variables
volatile
int32_t
u
=
0
;
// 11 frac bits
volatile
int32_t
v
=
0
;
// 11 frac bits
volatile
int32_t
vSat
=
0
;
volatile
int8_t
brake
=
0
;
// brake variable if pos-sample missed
volatile
int32_t
I
=
0
;
// 11 frac bits
volatile
int32_t
e
=
0
;
// 11 frac bits
volatile
int8_t
intCond
=
0
;
volatile
int32_t
K
=
807
;
// 6 frac bits, prop constant
volatile
int32_t
Ke
=
13
;
// 6 frac bits, integral constant
...
...
@@ -133,14 +140,23 @@ int32_t getCurrentRef() {
/* Set new acceleration reference value */
void
setAccRef
(
int32_t
newAccRef
)
{
g_acc_ref
=
newAccRef
;
nbrSamples
=
0
;
// Called from serial interrupt, so should be atomic by itself
if
(
ref
.
vel
.
pending
)
{
ref
.
vel
.
pending
=
0
;
ref
.
vel
.
value
=
ref
.
vel
.
next
;
ref
.
acc
.
value
=
newAccRef
;
ref
.
samples
=
0
;
}
else
{
// TODO: report error
}
}
/* Set new velocity reference value */
void
setVelRef
(
int32_t
newVelRef
)
{
g_vel_ref
=
newVelRef
;
// Called from serial interrupt, so should be atomic by itself
ref
.
vel
.
pending
=
1
;
ref
.
vel
.
next
=
newVelRef
;
}
/* Routine used to initialize the positional encoders */
...
...
@@ -246,64 +262,67 @@ SIGNAL(TWI_vect) {
/* Timer 0, control loop , 1 kHz */
SIGNAL
(
TIMER1_COMPA_vect
)
{
posCtrl
=
pos
;
// store pos to use in this loop
int32_t
vel_ref
=
g_vel_ref
;
int32_t
acc_ref
=
g_acc_ref
;
if
(
nbrSamples
<
65535
)
{
nbrSamples
++
;
if
(
ref
.
samples
<=
500
)
{
ref
.
samples
++
;
}
sei
();
// to enable interupts from encoder counter and TWI
posCtrl
=
pos
;
// store pos to use in this loop
int32_t
vel_ref
=
ref
.
vel
.
value
;
int32_t
acc_ref
=
ref
.
acc
.
value
;
int16_t
samples
=
ref
.
samples
;
sei
();
// to enable interupts from encoder counter, serial and TWI
// velocity estimation in mm/s
velEst
=
(((
a
*
velEst
+
64
)
>>
7
)
+
b
*
(
posCtrl
-
oldPos
));
// 5 fracbits on velEst
oldPos
=
posCtrl
;
// store velEst and ref to be sent/used here
// cli();
refCtrl
=
vel_ref
+
((
acc_ref
*
nbrSamples
)
>>
10
);
//shift nbrSamples 10 steps (= nbrSamples*h)
if
(
nbrSamples
>
500
)
{
refCtrl
=
0
;
// for safety reasons if doesn't send anything in 0.5 s
}
// vel_ref = vel_ref*(1-brake); // emergency stop
// control error
e
=
refCtrl
-
((
velEst
+
16
)
>>
5
);
// mm/s
if
(
samples
>
500
)
{
// Communication lost, stop and reset
I
=
0
;
// Protect u and TWCR for concurrent updates
cli
();
u
=
0
;
}
else
{
// store velEst and ref to be sent/used here
refCtrl
=
vel_ref
+
((
acc_ref
*
samples
)
>>
10
);
//shift nbrSamples 10 steps (= nbrSamples*h)
// control error
int32_t
e
=
refCtrl
-
((
velEst
+
16
)
>>
5
);
// mm/s
// temporary ctrl-signal
v
=
(((
K
*
e
+
(
1
<<
5
))
>>
6
)
+
((
I
+
(
1
<<
3
))
>>
4
));
// temporary ctrl-signal
int32_t
v
=
(((
K
*
e
+
(
1
<<
5
))
>>
6
)
+
((
I
+
(
1
<<
3
))
>>
4
));
// friction compensation
if
(
refCtrl
>
0
)
{
v
=
v
+
fr_comp
;
}
else
if
(
refCtrl
<
0
)
{
v
=
v
-
fr_comp
;
}
// friction compensation
if
(
refCtrl
>
0
)
{
v
=
v
+
fr_comp
;
}
else
if
(
refCtrl
<
0
)
{
v
=
v
-
fr_comp
;
}
// variable that decides if I-part should be updated
intCond
=
1
;
// variable that decides if I-part should be updated
int8_t
intCond
=
1
;
// saturation of v
if
(
v
>
V_MAX
)
{
vSat
=
V_MAX
;
if
(
e
>
0
)
intCond
=
0
;
}
else
if
(
v
<
V_MIN
)
{
vSat
=
V_MIN
;
if
(
e
<
0
)
intCond
=
0
;
}
else
{
vSat
=
v
;
}
// saturation of v
int32_t
vSat
;
if
(
v
>
V_MAX
)
{
vSat
=
V_MAX
;
if
(
e
>
0
)
{
intCond
=
0
;
}
}
else
if
(
v
<
V_MIN
)
{
vSat
=
V_MIN
;
if
(
e
<
0
)
{
intCond
=
0
;
}
}
else
{
vSat
=
v
;
}
if
(
intCond
)
I
=
I
+
(((
Ke
*
e
)
+
(
1
<<
1
))
>>
2
);
// Protect u and TWCR for concurrent updates
cli
();
// scale ctrl-signal to send over twi
u
=
(
vSat
+
8
)
>>
4
;
// u=127 gives current = 6.75 A, vSat makes u saturate at 114
if
(
intCond
)
{
I
=
I
+
(((
Ke
*
e
)
+
(
1
<<
1
))
>>
2
);
}
// Protect u and TWCR for concurrent updates
cli
();
// scale ctrl-signal to send over twi
u
=
(
vSat
+
8
)
>>
4
;
// u=127 gives current = 6.75 A, vSat makes u saturate at 114
}
// TWI-communication to set current reference on the other atmel
// send start command
...
...
@@ -353,15 +372,9 @@ int main()
//Serial communication initialization
UCSRA
=
0x00
;
// USART:
UCSRB
=
0x18
;
// USART: RxEnable|TxEnable
UCSRC
=
0x86
;
// USART: 8bit, no parity
UBRRH
=
0x00
;
// USART: 115200 @ 14.7456MHz
UBRRL
=
7
;
// USART: 115200 @ 14.7456MHz
UCSRA
=
0x00
;
// USART:
UCSRB
=
0x98
;
// USART: RxIntEnable|RxEnable|TxEnable
UCSRC
=
0x86
;
// USART: 8bit, no parity
, 9600
UCSRC
=
0x86
;
// USART: 8bit, no parity
UBRRH
=
0x0
;
// USART: 115200 @ 14.7456MHz
UBRRL
=
7
;
// USART: 115200 @ 14.7456MHz
...
...
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