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
4cf532d5
Commit
4cf532d5
authored
Feb 06, 2009
by
Pontus Giselsson
Browse files
Working velocity control without simulink communication
parent
c5f34060
Changes
1
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/vel_control.c
0 → 100644
View file @
4cf532d5
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>
// reference variables
volatile
int32_t
ref
=
0
;
// 11 frac bits
volatile
int16_t
refFlag
=
0
;
volatile
int16_t
deltaRef
=
2
;
volatile
int16_t
refCount
=
0
;
// velocity control variables
volatile
int32_t
u
=
0
;
// 11 frac bits
volatile
int32_t
v
=
0
;
// 11 frac bits
volatile
int8_t
brake
=
0
;
volatile
int32_t
I
=
0
;
// 11 frac bits
volatile
int32_t
e
=
0
;
// 11 frac bits
volatile
int32_t
K
=
1200
;
// 6 frac bits
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
volatile
int16_t
pos
=
0
;
volatile
int16_t
oldPos
=
0
;
volatile
int16_t
deltaPos
=
0
;
volatile
int8_t
newX
;
volatile
int8_t
newY
;
volatile
int8_t
oldX
;
volatile
int8_t
oldY
;
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
()
{
}
/* Routine used to set the red LED */
void
setLED
(
uint8_t
on
)
{
if
(
on
)
PORTB
&=
~
0x80
;
//Turn on
else
PORTB
|=
0x80
;
//Turn off
}
/* Routine used to initialize the positional encoders */
void
initPos
()
{
oldX
=
ENCODERX
;
oldY
=
ENCODERY
;
}
/* Routine used to track the cart position */
void
setPos
()
{
}
/* Timer 2, Encoder */
SIGNAL
(
SIG_OUTPUT_COMPARE2
)
{
deltaPos
=
0
;
// Update position from encoder
newX
=
ENCODERX
;
newY
=
ENCODERY
;
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
;
}
else
if
(
sum
==
1
||
sum
==
7
||
sum
==
8
||
sum
==
14
)
{
//pos = pos-1;
deltaPos
=
-
1
;
}
else
{
brake
=
1
;
// emergency brake
}
oldX
=
newX
;
oldY
=
newY
;
}
// velocity estimation cut-off frequency 500 Hz
pos
=
pos
+
deltaPos
;
// update oldPos, 0 frac bits on pos and oldPos
}
/* Timer 0, control loop */
SIGNAL
(
SIG_OUTPUT_COMPARE0
)
{
sei
();
// to enable interupts from timer2
PORTC
|=
0x10
;
// to clock calulation time
// linear velocity estimator
// velocity estimate in mm/s
velEst
=
(((
a
*
velEst
+
64
)
>>
7
)
+
b
*
(
pos
-
oldPos
));
// 5 fracbits on velEst
oldPos
=
pos
;
// control error
//e = ref-((int16_t)((velEst+16)>>5)); // mm/s
e
=
ref
-
((
velEst
+
16
)
>>
5
);
// mm/s
v
=
(((
K
*
e
+
(
1
<<
5
))
>>
6
)
+
((
I
+
(
1
<<
3
))
>>
4
));
//saturation and update integral part of ctrl
if
(
v
>
2047
)
{
I
=
I
+
((((
Ke
*
e
)
+
(
Ksat
)
*
(
2047
-
v
))
+
(
1
<<
1
))
>>
2
);
}
else
if
(
v
<
-
2048
)
{
I
=
I
+
((((
Ke
*
e
)
+
(
Ksat
)
*
(
-
2048
-
v
))
+
(
1
<<
1
))
>>
2
);
}
else
{
I
=
I
+
((
Ke
*
e
+
(
1
<<
3
))
>>
2
);
}
u
=
(
v
+
8
)
>>
4
;
//8 frac bits to current loop
// friction compensation
if
(
ref
>
0
)
{
u
=
u
+
10
;
}
else
if
(
ref
<
0
)
{
u
=
u
-
10
;
}
// Saturation
if
(
u
>
127
)
{
u
=
127
;
}
else
if
(
u
<
-
128
)
{
u
=
-
128
;
}
// reference calculations
refCount
++
;
if
(
refFlag
==
0
)
{
if
(
refCount
%
1
==
0
)
ref
=
ref
+
deltaRef
;
if
(
refCount
==
500
)
{
refFlag
=
1
;
deltaRef
=
-
deltaRef
;
refCount
=
0
;
}
}
else
{
if
(
refCount
%
1
==
0
)
ref
=
ref
+
deltaRef
;
if
(
refCount
==
1000
)
{
refCount
=
0
;
deltaRef
=
-
deltaRef
;
}
}
/*
if (refCount == 1000) {
ref = 0;
}
*/
pos
=
pos
*
(
1
-
brake
);
// TWI-communication
// 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
outp
((
int8_t
)(
u
&
0x000000ff
),
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
);
//velEstTemp = velEst;
/*
putchar((unsigned char) ((((velEstTemp+16)>>5)&0x0000ff00)>>8));
putchar((unsigned char) (((velEstTemp+16)>>5)&0x000000ff));
//putchar((unsigned char) ((deltaPos&0xff00)>>8));
//putchar((unsigned char) (deltaPos&0x00ff));
*/
putchar
((
unsigned
char
)
((
velEst
&
0xff000000
)
>>
24
));
putchar
((
unsigned
char
)
((
velEst
&
0x00ff0000
)
>>
16
));
putchar
((
unsigned
char
)
((
velEst
&
0x0000ff00
)
>>
8
));
putchar
((
unsigned
char
)
(
velEst
&
0x000000ff
));
putchar
((
unsigned
char
)
((
I
&
0xff000000
)
>>
24
));
putchar
((
unsigned
char
)
((
I
&
0x00ff0000
)
>>
16
));
putchar
((
unsigned
char
)
((
I
&
0x0000ff00
)
>>
8
));
putchar
((
unsigned
char
)
(
I
&
0x000000ff
));
PORTC
&=
~
0x10
;
}
int
main
()
{
cli
();
//Port directions
outp
(
0x80
,
DDRB
);
// Led output
outp
(
0x10
,
DDRC
);
// timer calculation port
/* Timer section */
// Enable timer2 compare match interrupts
outp
(
BV
(
OCIE0
)
|
BV
(
OCIE2
),
TIMSK
);
/* Timer 2, 59 kHz Prescaler 1 */
outp
(
BV
(
WGM21
)
|
BV
(
CS20
),
TCCR2
);
outp
(
200
,
OCR2
);
/* Reset timer 2 */
outp
(
0
,
TCNT2
);
/* Timer 0, 1 kHz Prescaler 64 */
outp
(
BV
(
WGM01
)
|
BV
(
CS01
)
|
BV
(
CS00
),
TCCR0
);
outp
(
230
,
OCR0
);
/* Reset timer 0 */
outp
(
0
,
TCNT0
);
//Serial communication
outp
(
0x00
,
UCSRA
);
// USART:
outp
(
0x98
,
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);
// Enable ADC, start first conversion, prescaler 32, not free running mode
// outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Initialize Master TWI
outp
(
0x10
,
TWBR
);
// set SCL-frequency CPU-freq/(16+2*16)
outp
(
BV
(
TWEN
),
TWCR
);
// enable TWI
// initialize position measurements
initPos
();
//Enable interrupts
sei
();
// loop
while
(
1
)
{}
}
Write
Preview
Markdown
is supported
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