Skip to content
GitLab
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
63447c91
Commit
63447c91
authored
Feb 10, 2009
by
Pontus Giselsson
Browse files
Velocity controller with slow simulink communication, program structure must be rebuilt.
parent
37d6b571
Changes
3
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/current_control_final.c
View file @
63447c91
...
...
@@ -6,14 +6,14 @@
// control variables
volatile
int16_t
ref
=
0
;
volatile
int16_t
ref
=
0
;
// reference value (from velocity controller)
volatile
unsigned
char
lbyte
,
hbyte
;
volatile
int16_t
y
;
volatile
int16_t
y
;
// measurement, 9 frac bits
volatile
uint8_t
low
,
high
;
volatile
int16_t
e
=
0
;
volatile
int16_t
v
=
0
;
volatile
int16_t
I
=
0
;
volatile
int16_t
u
=
0
;
volatile
int16_t
e
=
0
;
// control error, 9 frac bits
volatile
int16_t
v
=
0
;
// temporary ctrl signal, 9 frac bits
volatile
int16_t
I
=
0
;
// integral part of ctrl, 13 frac bits
volatile
int16_t
u
=
0
;
// ctrl signal = pwm high time
volatile
int16_t
K
=
74
;
// 7 frac bits
volatile
int16_t
Ke
=
26
;
//7 frac bits, K*h/Ti
volatile
int16_t
Ksat
=
44
;
// 7 frac bits, h/Tr
...
...
@@ -33,31 +33,46 @@ volatile int8_t stop = 0;
volatile
int16_t
status
;
/* Interrupt service routine for handling incoming bytes on the serial port
might be needed to catch incoming bytes */
SIGNAL
(
SIG_UART_RECV
){}
/* Send logged data over Serial connection */
static
inline
void
sendData
()
{
int16_t
ii
=
0
;
while
(
ii
<
log_len
)
{
putchar
((
unsigned
char
)
((
ctrl_log
[
ii
]
&
0xff00
)
>>
8
));
putchar
((
unsigned
char
)
(
ctrl_log
[
ii
]
&
0x00ff
));
/* Timer 1 compare match, start AD-conversion
* Additional changes: Dont use free running AD-conversion
* Enable overflow interrupts timer1
*/
putchar
((
unsigned
char
)
((
error_log
[
ii
]
&
0xff00
)
>>
8
));
putchar
((
unsigned
char
)
(
error_log
[
ii
]
&
0x00ff
));
putchar
((
unsigned
char
)
((
I_log
[
ii
]
&
0xff000000
)
>>
24
));
putchar
((
unsigned
char
)
((
I_log
[
ii
]
&
0x00ff0000
)
>>
16
));
putchar
((
unsigned
char
)
((
I_log
[
ii
]
&
0x0000ff00
)
>>
8
));
putchar
((
unsigned
char
)
(
I_log
[
ii
]
&
0x000000ff
));
ii
++
;
}
}
/* Timer 2 compare match interupt */
SIGNAL
(
SIG_OUTPUT_COMPARE2
)
{
// Start AD conversion
ADCSRA
|=
BV
(
ADSC
);
PORTB
|=
0x40
;
//Turn on calculation timer
// Read input
low
=
inp
(
ADCL
);
high
=
inp
(
ADCH
);
y
=
((
high
<<
8
)
|
low
)
-
512
;
//y 9 frac bits
//control
,
since negative measurements
//control
error, (+)
since negative measurements
e
=
ref
+
y
;
//e 9 frac bits
//v = (int16_t)((K*e+I+64)>>7);
v
=
(
int16_t
)(((
K
*
e
+
64
)
>>
7
)
+
(
I
>>
4
));
//saturation and update integral part of ctrl
if
(
v
>
511
)
{
I
=
I
+
(((
Ke
*
e
)
+
(
Ksat
)
*
(
511
-
v
))
>>
3
);
...
...
@@ -67,15 +82,16 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
I
=
I
+
((
Ke
*
e
)
>>
3
);
}
//
Original styrning med
7 bit
ar
+ direction
//
ctrl signal,
7 bit
s
+ direction
u
=
(
v
+
2
)
>>
2
;
//7 frac bits to pwm
// saturation of ctrl-signal
if
(
u
>
127
)
{
u
=
127
;
}
else
if
(
u
<
-
128
)
{
u
=
-
128
;
}
//u = 0;
// set pwm switching time
if
(
u
<
0
)
{
PORTC
|=
0x80
;
...
...
@@ -85,7 +101,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
OCR1BL
=
(
unsigned
char
)
(
127
-
u
);
}
// TWI-communication
(need status == 0x60?)
// TWI-communication
, recieve reference from velocity controller
if
((
BV
(
TWINT
)
&
inp
(
TWCR
)))
{
status
=
(
inp
(
TWSR
)
&
0xf8
);
if
(
status
==
0x80
)
{
...
...
@@ -97,27 +113,39 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
outp
(
BV
(
TWINT
)
|
BV
(
TWEN
)
|
BV
(
TWEA
),
TWCR
);
}
PORTB
&=
0xbf
;
//Turn off calculation timer
// For logging purposes
/*
countSamples++;
if (countSamples == skipSamples) {
ctrl_log[jj] = u;
I_log[jj] = I;
error_log[jj] = e;
jj++;
countSamples = 0;
// Stop after a while
if ((jj == (log_len-1)) & !stop) {
outp(0x7f,OCR1BL);
stop = 1;
sendData();
}
}
*/
}
int
main
()
{
// clear interrupts
cli
();
//Port directions
outp
(
0x08
,
PORTC
);
// pull up on overtemp signals
outp
(
0xa0
,
DDRC
);
// output on dir and brake
outp
(
0x10
,
DDRD
);
// output on pwm
for motor 1
outp
(
0x10
,
DDRD
);
// output on pwm
outp
(
0x40
,
DDRB
);
// output on timer calculation pin
outp
(
0x10
,
DDRA
);
// test pin output
/* Timer section */
// Enable timer2 compare match interrupts
outp
(
BV
(
OCIE2
),
TIMSK
);
// Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high))
outp
(
BV
(
COM1A1
)
|
BV
(
COM1B1
)
|
BV
(
COM1A0
)
|
BV
(
COM1B0
)
|
BV
(
WGM11
)
|
BV
(
WGM10
),
TCCR1A
);
...
...
@@ -137,15 +165,17 @@ int main()
outp
(
0x3f
,
OCR2
);
/* Reset timer 2 */
outp
(
0
,
TCNT2
);
// Enable timer2 compare match interrupts
outp
(
BV
(
OCIE2
),
TIMSK
);
//Serial communication
outp
(
0x00
,
UCSRA
);
// USART:
outp
(
0x
9
8
,
UCSRB
);
// USART: RxIntEnable|RxEnable|TxEnable
outp
(
0x
1
8
,
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
/* AREF (AREF is 5V) pin external capacitor, MUX0 for current */
outp
(
BV
(
REFS0
)
|
BV
(
MUX0
),
ADMUX
);
...
...
@@ -158,9 +188,6 @@ int main()
outp
(
0x02
,
TWAR
);
// slave address
outp
(
BV
(
TWEA
)
|
BV
(
TWEN
),
TWCR
);
// enable TWI, enable ACK
//Enable interrupts
sei
();
...
...
linear_pendulum_2009/avr/pccom.c
View file @
63447c91
...
...
@@ -107,13 +107,6 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available
{
static
int8_t
configPosition
=
-
1
;
// static uint16_t toPoll = 0;
// // see if we should fetch new poll mask
// if (toPoll == 0) {
// toPoll = pccom_poll;
// pccom_poll = 0;
// if (toPoll == 0) return 0;
// }
#define toPoll pccom_poll // OK since sender and receiver are mutexed
// Send _first_ requested item (only one packet!)
...
...
linear_pendulum_2009/avr/vel_control.c
View file @
63447c91
...
...
@@ -31,6 +31,7 @@ volatile int32_t Ksat = 3; // 6 frac bits
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<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
;
...
...
@@ -42,33 +43,14 @@ volatile int8_t sum = 0;
// velocity estimation parameters
volatile
int32_t
velEst
=
0
;
// 5 frac bits
volatile
int32_t
velEstTemp
=
0
;
volatile
int16_t
posTemp
=
0
;
int16_t
a
=
116
;
//7 frac bits
int16_t
b
=
152
;
// 5 frac bits
/* 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
)
{};
}
/* Interrupt service routine for handling incoming bytes on the serial port
might be needed to catch incoming bytes */
SIGNAL
(
SIG_UART_RECV
){}
/* Send logged data over Serial connection */
static
inline
void
sendData
()
{
}
// return position (in tics)
int32_t
getPosition
()
{
return
pos
;
posTemp
=
pos
;
return
((
int32_t
)
posTemp
);
}
...
...
@@ -141,6 +123,8 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
sei
();
// to enable interupts from timer2
TIMSK
&=
~
BV
(
OCIE0
);
PORTC
|=
0x10
;
// to clock calulation time
// linear velocity estimator
...
...
@@ -214,6 +198,7 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
ref
=
ref
*
(
1
-
brake
);
// TWI-communication
// send start command
outp
(
BV
(
TWINT
)
|
BV
(
TWEN
)
|
BV
(
TWSTA
),
TWCR
);
...
...
@@ -236,32 +221,41 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
// stop transmission
outp
(
BV
(
TWINT
)
|
BV
(
TWEN
)
|
BV
(
TWSTO
),
TWCR
);
// ------- Noncritical section -------
// 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;
while
(
toSend
>=
0
)
{
UDR
=
(
char
)
toSend
;
while
((
UCSRA
&
(
1
<<
UDRE
))
==
0
)
{}
// send all data in buffer
toSend
=
pccom_getNextByteToSend
();
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
;
PORTC
&=
~
0x10
;
TIMSK
|=
BV
(
OCIE0
);
}
...
...
@@ -300,10 +294,10 @@ int main()
/* AREF (AREF is 5V) pin external capacitor, MUX0 for current */
//
outp(BV(REFS0)|BV(MUX
0
),ADMUX);
//outp(BV(REFS0)|BV(MUX
3
),ADMUX);
// Enable ADC, start first conversion, prescaler 32,
not
free running mode
//
outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC, start first conversion, prescaler 32, free running mode
//outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Initialize Master TWI
...
...
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment