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
45ce6676
Commit
45ce6676
authored
May 11, 2015
by
Anders Blomdell
Browse files
Change to new style I/O
Make TWI communication interrupt controlled
parent
cf36849b
Changes
1
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/vel_control.c
View file @
45ce6676
...
...
@@ -10,7 +10,6 @@
#include
<avr/io.h>
#include
<avr/interrupt.h>
#include
<inttypes.h>
#include
<compat/deprecated.h>
#include
"vel_control.h"
#include
"serialio.h"
...
...
@@ -44,16 +43,16 @@
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
vel_ref
=
0
;
// 11 frac bits (variable in mm/s, max 2^12)
volatile
int32_t
g_
vel_ref
=
0
;
// 11 frac bits (variable in mm/s, max 2^12)
volatile
int32_t
refCtrl
=
0
;
// ref used in ctrl-loop (=ref sent from simulink)
volatile
int32_t
acc_ref
=
0
;
// 14 frac bits (variable in mm/s, max 2^15)
volatile
uint16_t
nbrSamples
=
0
;
// nbr of samples between ctrl-ref updates
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
uSend
=
0
;
// ctrl sent to simulink
volatile
int32_t
v
=
0
;
// 11 frac bits
volatile
int32_t
vSat
=
0
;
volatile
int8_t
brake
=
0
;
// brake variable if pos-sample missed
...
...
@@ -62,7 +61,7 @@ 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
K
=
807
;
// 6 frac bits, prop constant
volatile
int32_t
Ke
=
13
;
// 6 frac bits, integral constant
volatile
int8_t
fr_comp
=
(
10
<<
3
);
#define V_MAX (120<<4)
...
...
@@ -84,7 +83,6 @@ volatile int8_t oldY; // previous encoder signal
// velocity estimation parameters
volatile
int32_t
velEst
=
0
;
// vel-estimate, 5 frac bits
volatile
int32_t
velEstSend
=
0
;
// vel-estimate sent to simulink
int16_t
a
=
116
;
// 7 frac bits (parameter in velocity estimate)
int16_t
b
=
152
;
// 5 frac bits (parameter in velocity estimate)
...
...
@@ -105,35 +103,44 @@ int32_t getPosition() {
/* return velocity (in mm/s) */
int32_t
getVelocity
()
{
return
velEstSend
;
int32_t
result
;
cli
();
result
=
velEst
;
sei
();
return
result
;
}
/* return last angle measurement */
int16_t
getAngle
()
{
low
=
inp
(
ADCL
);
high
=
inp
(
ADCH
);
int16_t
result
=
ADC
-
512
-
angleOffset
;
return
result
;
int16_t
adc
=
ADC
;
while
(
adc
!=
ADC
)
{
adc
=
ADC
;
}
return
adc
-
512
-
angleOffset
;
}
/* return current-reference */
int32_t
getCurrentRef
()
{
return
uSend
;
int32_t
result
;
cli
();
result
=
u
;
sei
();
return
result
;
}
/* Set new acceleration reference value */
void
setAccRef
(
int32_t
newAccRef
)
{
acc_ref
=
newAccRef
;
g_
acc_ref
=
newAccRef
;
nbrSamples
=
0
;
}
/* Set new velocity reference value */
void
setVelRef
(
int32_t
newVelRef
)
{
vel_ref
=
newVelRef
;
g_
vel_ref
=
newVelRef
;
}
/* Routine used to initialize the positional encoders */
...
...
@@ -149,26 +156,26 @@ SIGNAL(TIMER2_COMP_vect) {
newX
=
ENCODERX
;
newY
=
ENCODERY
;
if
((
newX
!=
oldX
)
||
(
newY
!=
oldY
))
{
//Check if any value changed
/*
/*
sum = (oldX<<2)+oldY+newX+(newY>>2);
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
pos = pos+1;
pos = pos+1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
pos = pos-1;
pos = pos-1;
} else {
brake = 1; // emergency brake
brake = 1; // emergency brake
}
*/
*/
// Works like if-clauses above, but faster!
if
((
oldX
==
newY
)
&&
(
oldY
!=
newX
))
{
pos
=
pos
+
1
;
}
else
if
((
oldX
!=
newY
)
&&
(
oldY
==
newX
))
{
pos
=
pos
-
1
;
}
else
{
brake
=
1
;
}
oldX
=
newX
;
oldY
=
newY
;
if
((
oldX
==
newY
)
&&
(
oldY
!=
newX
))
{
pos
=
pos
+
1
;
}
else
if
((
oldX
!=
newY
)
&&
(
oldY
==
newX
))
{
pos
=
pos
-
1
;
}
else
{
brake
=
1
;
}
oldX
=
newX
;
oldY
=
newY
;
}
}
...
...
@@ -214,59 +221,50 @@ SIGNAL(USART_RXC_vect) {
}
}
#if 0
/* Timer 0, serial communication with simulink */
SIGNAL(TIMER0_COMP_vect) {
TIMSK &= ~(_BV(OCIE0)|_BV(OCIE1A)); // Disable communication and ctrl-interrupts
sei(); // enable interrupts from encoder-counter
// Poll UART receiver
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
pccom_receiveByte(ch);
if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) {
//main_emergencyStop(); // stop on USART error
}
}
// Poll UART sender
if (UCSRA & (1<<UDRE)) {
int16_t toSend = pccom_getNextByteToSend();
if (toSend >= 0) UDR = (char)toSend;
SIGNAL
(
TWI_vect
)
{
unsigned
char
twsr
=
TWSR
;
switch
(
twsr
)
{
case
0x08
:
{
TWDR
=
0x02
;
// slave is 0x02 (sla+w)
TWCR
=
_BV
(
TWINT
)
|
_BV
(
TWEN
)
|
_BV
(
TWIE
);
}
break
;
case
0x18
:
case
0x20
:
{
TWDR
=
u
;
TWCR
=
_BV
(
TWINT
)
|
_BV
(
TWEN
)
|
_BV
(
TWIE
);
}
break
;
case
0x28
:
case
0x30
:
{
TWCR
=
_BV
(
TWINT
)
|
_BV
(
TWEN
)
|
_BV
(
TWSTO
)
|
_BV
(
TWIE
);
}
break
;
default:
{
// This should never happen
TWCR
=
(
_BV
(
TWINT
)
|
_BV
(
TWEN
)
|
_BV
(
TWIE
));
}
break
;
}
TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl)
TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts
}
#endif
/* Timer 0, control loop , 1 kHz */
SIGNAL
(
TIMER1_COMPA_vect
)
{
posCtrl
=
pos
;
// store pos to use in this loop
sei
();
// to enable interupts from encoder counter and communication
int32_t
vel_ref
=
g_vel_ref
;
int32_t
acc_ref
=
g_acc_ref
;
if
(
nbrSamples
<
65535
)
{
nbrSamples
++
;
}
sei
();
// to enable interupts from encoder counter 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
();
velEstSend
=
velEst
;
nbrSamples
++
;
// 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
sei
();
// control error
e
=
refCtrl
-
((
velEst
+
16
)
>>
5
);
// mm/s
...
...
@@ -300,32 +298,18 @@ SIGNAL(TIMER1_COMPA_vect) {
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
// u that is sent to simulink
cli
();
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
);
while
(
!
(
TWCR
&
_BV
(
TWINT
)))
{}
// Send reference byte to slave
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
);
if
(
TWCR
==
(
_BV
(
TWEN
)
|
_BV
(
TWIE
))
||
TWCR
==
0
)
{
TWCR
=
_BV
(
TWEN
)
|
_BV
(
TWSTA
)
|
_BV
(
TWIE
);
}
}
...
...
@@ -334,36 +318,35 @@ int main()
cli
();
//Port directions
//outp(0xf0,DDRB); // Led output
outp
(
0x10
,
DDRC
);
// timer calculation port
PORTC
=
0x03
;
// Pull up for systems without TWI resistors
PORTD
=
0x40
;
// pull up on reset switch
/* Timer section */
// Enable timer0, timer1, timer2 compare match interrupts
outp
(
_BV
(
OCIE0
)
|
_BV
(
OCIE1A
)
|
_BV
(
OCIE2
),
TIMSK
);
outp
(
_BV
(
OCIE1A
)
|
_BV
(
OCIE2
),
TIMSK
);
// Enable timer1, timer2 compare match interrupts
TIMSK
=
_BV
(
OCIE1A
)
|
_BV
(
OCIE2
);
/* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
outp
(
_BV
(
WGM21
)
|
_BV
(
CS20
),
TCCR2
);
outp
(
200
,
OCR2
);
TCCR2
=
_BV
(
WGM21
)
|
_BV
(
CS20
);
OCR2
=
200
;
/* Reset timer 2 */
outp
(
0
,
TCNT2
)
;
TCNT2
=
0
;
/* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
outp
(
_BV
(
WGM12
)
|
_BV
(
CS10
)
,
TCCR1B
)
;
outp
(
0x38
,
OCR1AH
)
;
outp
(
0x3f
,
OCR1AL
)
;
outp
(
0
,
TCNT1H
)
;
outp
(
0
,
TCNT1L
)
;
TCCR1B
=
_BV
(
WGM12
)
|
_BV
(
CS10
);
OCR1AH
=
0x38
;
OCR1AL
=
0x3f
;
TCNT1H
=
0
;
TCNT1L
=
0
;
/* Timer 0, 40 kHz, Prescaler 8, serial communication */
outp
(
_BV
(
WGM01
)
|
_BV
(
CS01
)
,
TCCR0
)
;
//
outp(184-1,OCR0)
; // 10 kHz
outp
(
45
-
1
,
OCR0
)
;
// 40 kHz
TCCR0
=
_BV
(
WGM01
)
|
_BV
(
CS01
);
//
OCR0 = 184-1
; // 10 kHz
OCR0
=
45
-
1
;
// 40 kHz
/* Reset timer 0 */
outp
(
0
,
TCNT0
)
;
TCNT0
=
0
;
// syncronization (ctrl interrupt 34 micros before communication interrupt)
...
...
@@ -371,11 +354,11 @@ int main()
//Serial communication initialization
outp
(
0x00
,
UCSRA
)
;
// USART:
outp
(
0x18
,
UCSRB
)
;
// USART: RxEnable|TxEnable
outp
(
0x86
,
UCSRC
)
;
// USART: 8bit, no parity
outp
(
0x00
,
UBRRH
)
;
// USART: 115200 @ 14.7456MHz
outp
(
7
,
UBRRL
)
;
// USART: 115200 @ 14.7456MHz
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
...
...
@@ -385,15 +368,15 @@ int main()
/* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
outp
(
_BV
(
REFS0
)
|
_BV
(
MUX0
)
|
_BV
(
MUX1
)
,
ADMUX
)
;
ADMUX
=
_BV
(
REFS0
)
|
_BV
(
MUX0
)
|
_BV
(
MUX1
);
// Enable ADC on adc3, start first conversion, prescaler 128, free running mode
outp
(
_BV
(
ADEN
)
|
_BV
(
ADSC
)
|
_BV
(
ADATE
)
|
_BV
(
ADPS2
)
|
_BV
(
ADPS1
)
|
_BV
(
ADPS0
)
,
ADCSRA
)
;
ADCSRA
=
_BV
(
ADEN
)
|
_BV
(
ADSC
)
|
_BV
(
ADATE
)
|
_BV
(
ADPS2
)
|
_BV
(
ADPS1
)
|
_BV
(
ADPS0
);
// Initialize Master TWI
outp
(
0x10
,
TWBR
)
;
// set SCL-frequency CPU-freq/(16+2*16)
outp
(
_BV
(
TWEN
)
,
TWCR
);
// enable TWI
TWBR
=
0x10
;
// set SCL-frequency CPU-freq/(16+2*16)
TWCR
=
_BV
(
TWEN
)
|
_BV
(
TWIE
);
// initialize position measurements
initPos
();
...
...
@@ -404,6 +387,7 @@ int main()
//Enable interrupts
sei
();
// loop
while
(
1
)
{
...
...
@@ -411,8 +395,8 @@ int main()
// reset position, velocity estimate and integral part of ctrl if reset button pushed
if
(
!
(
PIND
&
0x40
))
{
cli
();
low
=
inp
(
ADCL
)
;
high
=
inp
(
ADCH
)
;
low
=
ADCL
;
high
=
ADCH
;
pos
=
0
;
// reset position
angleOffset
=
((
int16_t
)
((
high
<<
8
)
|
low
)
-
512
);
oldPos
=
0
;
...
...
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