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
36135468
Commit
36135468
authored
Feb 10, 2009
by
Pontus Giselsson
Browse files
Velocity control with low jitter on simulink communication
parent
63447c91
Changes
1
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/vel_control.c
View file @
36135468
...
...
@@ -28,12 +28,14 @@ volatile int32_t Ke = 45; // 6 frac bits
volatile
int32_t
Ksat
=
3
;
// 6 frac bits
// encoder variables
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
//#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
//#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
#define ENCODERY (PIND&0x04) //Positional encoder pins
#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins
volatile
int16_t
pos
=
0
;
volatile
int16_t
posTemp
=
0
;
volatile
int16_t
oldPos
=
0
;
volatile
int16_t
deltaPos
=
0
;
volatile
int8_t
newX
;
volatile
int8_t
newY
;
volatile
int8_t
oldX
;
...
...
@@ -86,46 +88,81 @@ void setPos()
}
/* Timer 2, Encoder */
/* Timer 2, Encoder
counter, 73 kHz
*/
SIGNAL
(
SIG_OUTPUT_COMPARE2
)
{
deltaPos
=
0
;
// Update position from encoder
newX
=
ENCODERX
;
newY
=
ENCODERY
;
if
((
newX
!=
oldX
)
||
(
newY
!=
oldY
))
//Check if any value changed
{
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 = pos+1;
deltaPos
=
1
;
pos = pos+1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
//pos = pos-1;
deltaPos
=
-
1
;
pos = pos-1;
} else {
brake = 1; // emergency brake
}
*/
if
((
oldX
==
newY
)
&&
(
oldY
!=
newX
))
{
pos
=
pos
+
1
;
}
else
if
((
oldX
!=
newY
)
&&
(
oldY
==
newX
))
{
pos
=
pos
-
1
;
}
else
{
brake
=
1
;
// emergency brake
}
oldX
=
newX
;
oldY
=
newY
;
}
}
}
/* Timer 1, serial communication */
SIGNAL
(
SIG_OUTPUT_COMPARE0
)
{
TIMSK
&=
~
(
BV
(
OCIE1A
)
|
BV
(
OCIE0
));
sei
();
// enable interrupts from timer 0 and timer 2
//PORTC |= 0x10; // to clock calulation time
// velocity estimation cut-off frequency 500 Hz
pos
=
pos
+
deltaPos
;
// update oldPos, 0 frac bits on pos and oldPos
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
;
}
//PORTC &= ~0x10;
TIMSK
|=
(
BV
(
OCIE1A
)
|
BV
(
OCIE0
));
}
/* Timer 0, control loop */
SIGNAL
(
SIG_OUTPUT_COMPARE
0
)
{
/* Timer 0, control loop
, 1 kHz
*/
SIGNAL
(
SIG_OUTPUT_COMPARE
1A
)
{
TIMSK
&=
~
(
BV
(
OCIE0
)
|
BV
(
OCIE1A
));
sei
();
// to enable interupts from timer2
TIMSK
&=
~
BV
(
OCIE0
);
PORTC
|=
0x10
;
// to clock calulation time
// linear velocity estimator
// velocity estimate in mm/s
...
...
@@ -221,41 +258,12 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
// stop transmission
outp
(
BV
(
TWINT
)
|
BV
(
TWEN
)
|
BV
(
TWSTO
),
TWCR
);
TIMSK
|=
(
BV
(
OCIE0
)
|
BV
(
OCIE1A
));
// ------- Noncritical section -------
// Poll UART receiver
int8_t
tempVar
=
0
;
for
(
tempVar
=
0
;
tempVar
<
10
;
tempVar
++
)
{
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;
while
(
toSend
>=
0
)
{
UDR
=
(
char
)
toSend
;
while
((
UCSRA
&
(
1
<<
UDRE
))
==
0
)
{}
// wait for previous send
toSend
=
pccom_getNextByteToSend
();
}
}
}
PORTC
&=
~
0x10
;
TIMSK
|=
BV
(
OCIE0
);
}
...
...
@@ -268,22 +276,54 @@ int main()
outp
(
0x10
,
DDRC
);
// timer calculation port
/* Timer section */
// Enable timer2 compare match interrupts
outp
(
BV
(
OCIE0
)
|
BV
(
OCIE2
),
TIMSK
);
// Enable
timer0, timer1,
timer2 compare match interrupts
outp
(
BV
(
OCIE0
)
|
BV
(
OCIE1A
)
|
BV
(
OCIE2
),
TIMSK
);
/* Timer 2,
59
kHz Prescaler 1 */
/* Timer 2,
73
kHz Prescaler 1 */
outp
(
BV
(
WGM21
)
|
BV
(
CS20
),
TCCR2
);
outp
(
200
,
OCR2
);
/* Reset timer 2 */
outp
(
0
,
TCNT2
);
// --------old-----------------------------------------------
/* Timer 1, Prescaler 1, 73/8 kHz*/
//outp(BV(WGM12)|BV(CS10),TCCR1B);
//outp(0x06,OCR1AH);
//outp(0x40,OCR1AL);
//outp(0,TCNT1H);
//outp(0,TCNT1L);
/* Timer 0, 1 kHz Prescaler 64 */
//outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
//outp(230,OCR0);
/* Reset timer 0 */
//outp(0,TCNT0);
// --------new-----------------------------------------------
/* Timer 1, 1 kHz */
outp
(
BV
(
WGM12
)
|
BV
(
CS10
),
TCCR1B
);
outp
(
0x39
,
OCR1AH
);
outp
(
0x99
,
OCR1AL
);
outp
(
0
,
TCNT1H
);
outp
(
0
,
TCNT1L
);
/* Timer 0, 10 kHz, Prescaler 64 */
outp
(
BV
(
WGM01
)
|
BV
(
CS01
)
|
BV
(
CS00
),
TCCR0
);
outp
(
230
,
OCR0
);
//outp(200,OCR0);
outp
(
23
,
OCR0
);
// 10 kHz
/* Reset timer 0 */
outp
(
0
,
TCNT0
);
//----------------------------------------------------------
//Serial communication
outp
(
0x00
,
UCSRA
);
// USART:
outp
(
0x18
,
UCSRB
);
// USART: RxEnable|TxEnable
...
...
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