Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
P
processer
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Anders Blomdell
processer
Commits
45ce6676
Commit
45ce6676
authored
10 years ago
by
Anders Blomdell
Browse files
Options
Downloads
Patches
Plain Diff
Change to new style I/O
Make TWI communication interrupt controlled
parent
cf36849b
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
1
Show whitespace changes
Inline
Side-by-side
Showing
1 changed file
linear_pendulum_2009/avr/vel_control.c
+97
-113
97 additions, 113 deletions
linear_pendulum_2009/avr/vel_control.c
with
97 additions
and
113 deletions
linear_pendulum_2009/avr/vel_control.c
+
97
−
113
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
...
...
@@ -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 */
...
...
@@ -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
}
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
;
}
// Poll UART sender
if (UCSRA & (1<<UDRE)) {
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
}
#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
();
...
...
@@ -405,14 +388,15 @@ int main()
//Enable interrupts
sei
();
// loop
while
(
1
)
{
unsigned
char
to_poll
;
// 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
;
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment