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
37d6b571
Commit
37d6b571
authored
Feb 09, 2009
by
Pontus Giselsson
Browse files
Working velocity control with simulink communication
parent
4cf532d5
Changes
5
Hide whitespace changes
Inline
Side-by-side
linear_pendulum_2009/avr/pccom.c
0 → 100644
View file @
37d6b571
// pccom.c: Communication interface to PC via serialio_core.
#include
"pccom.h"
#include
"serialio_core.h"
#include
"vel_control.h"
/*
* Serial I/O assignments
*
* AO 0 -- Axis 1 motor voltage
* A0 1 -- Axis 1 velocity reference !
*
* EI 0 -- Axis 1 position !
* EI 1 -- Axis 1 filtered velocity !
* EI 5 -- Axis 1 position with predicted fraction
* EI 6 -- Axis 1 position unquantized
*
* AI 2 -- Axis 1 current
* AI 3 -- Pendulum angle !
*
* AI 4 -- Axis 1 motor voltage (actual)
*
* DI 0 -- Axis 1 endpoint sensor
*/
#define POLL_AXIS1_POSITION 0x0001
#define POLL_AXIS1_VELOCITY 0x0002
//#define POLL_PEND_ANGLE 0x0004
//#define POLL_AXIS1_RESET 0x0008
#define POLL_CONFIG 0x8000
static
volatile
uint16_t
pccom_poll
=
0
;
// ---------------------------------- Receiver --------------------------------
// only call from UART receive interrupt
static
inline
void
addPoll
(
uint16_t
flags
)
{
pccom_poll
|=
flags
;
}
void
pccom_receiveByte
(
char
ch
)
{
switch
(
serialio_RXC
(
ch
))
{
case
serialio_clearbit
:
{
switch
(
serialio_channel
)
{
}
}
break
;
case
serialio_setbit
:
{
switch
(
serialio_channel
)
{
}
}
break
;
case
serialio_pollbit
:
{
switch
(
serialio_channel
)
{
//case 0: { addPoll(POLL_AXIS1_RESET); } break;
}
}
break
;
case
serialio_pollchannel
:
{
switch
(
serialio_channel
)
{
case
0
:
{
addPoll
(
POLL_AXIS1_POSITION
);
}
break
;
case
1
:
{
addPoll
(
POLL_AXIS1_VELOCITY
);
}
break
;
//case 2: { addPoll(POLL_PEND_ANGLE); } break;
case
31
:
{
addPoll
(
POLL_CONFIG
);
}
break
;
}
}
break
;
case
serialio_setchannel
:
{
switch
(
serialio_channel
)
{
case
0
:
{
setRef
(
serialio_value
-
(
1L
<<
12
));
}
break
;
}
}
break
;
case
serialio_more
:
{
}
break
;
case
serialio_error
:
{
}
break
;
}
}
// ----------------------------------- Sender ------------------------------
// return true if more to send
static
uint8_t
sendConfigPacket
(
uint8_t
position
)
{
switch
(
position
)
{
case
0
:
CONF_ANALOG_IN
(
0
,
CONF_RESOLUTION
(
16
));
break
;
// Position (now reference)
case
1
:
CONF_ANALOG_IN
(
0
,
CONF_MIN
(
CONF_NEGATIVE_VOLT
(
1
)));
break
;
case
2
:
CONF_ANALOG_IN
(
0
,
CONF_MAX
(
CONF_POSITIVE_VOLT
(
1
)));
break
;
case
3
:
CONF_ANALOG_IN
(
1
,
CONF_RESOLUTION
(
18
));
break
;
// Velocity estimate
case
4
:
CONF_ANALOG_IN
(
1
,
CONF_MIN
(
CONF_NEGATIVE_VOLT
(
1
)));
break
;
case
5
:
CONF_ANALOG_IN
(
1
,
CONF_MAX
(
CONF_POSITIVE_VOLT
(
1
)));
break
;
case
6
:
CONF_ANALOG_OUT
(
0
,
CONF_RESOLUTION
(
13
));
break
;
// Reference to vel-ctrl
case
7
:
CONF_ANALOG_OUT
(
0
,
CONF_MIN
(
CONF_NEGATIVE_VOLT
(
1
)));
break
;
case
8
:
CONF_ANALOG_OUT
(
0
,
CONF_MAX
(
CONF_POSITIVE_VOLT
(
1
)));
break
;
default:
CONF_END
();
return
0
;
}
return
1
;
}
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!)
if
(
toPoll
&
POLL_AXIS1_POSITION
)
{
toPoll
&=
~
POLL_AXIS1_POSITION
;
serialio_putchannel
(
0
,
getPosition
()
+
(
1L
<<
15
));
}
else
if
(
toPoll
&
POLL_AXIS1_VELOCITY
)
{
toPoll
&=
~
POLL_AXIS1_VELOCITY
;
serialio_putchannel
(
1
,
getVelocity
()
+
(
1L
<<
17
));
}
else
if
(
toPoll
&
POLL_CONFIG
)
{
if
(
configPosition
<
0
)
configPosition
=
0
;
// Start sending config?
if
(
!
sendConfigPacket
(
configPosition
))
{
// Last packet?
configPosition
=
-
1
;
toPoll
&=
~
POLL_CONFIG
;
}
else
configPosition
++
;
// Advance to next packet
}
else
return
0
;
// should never happen!
return
1
;
}
// ---- Send buffering ----
#define PCCOM_SEND_BUFFER_SIZE 6 // just enough for one serialio_putchannel() packet
uint8_t
pccom_sendBuffer
[
PCCOM_SEND_BUFFER_SIZE
];
// Updated by serialio_putchar()
uint8_t
pccom_sendBufferPosition
=
0
;
// Updated by pccom_getNextByteToSend()
uint8_t
pccom_sendBufferUsed
=
0
;
// Updated by serialio_putchar(),
// and pccom_getNextByteToSend() when
// the buffer is empty.
void
serialio_putchar
(
unsigned
char
ch
)
{
if
(
pccom_sendBufferUsed
<
PCCOM_SEND_BUFFER_SIZE
)
{
pccom_sendBuffer
[
pccom_sendBufferUsed
]
=
ch
;
pccom_sendBufferUsed
++
;
}
else
{
// Buffer already full -- must never happen!
// main_emergencyStop(); // show that something is wrong
}
}
int16_t
pccom_getNextByteToSend
()
{
if
(
pccom_sendBufferPosition
>=
pccom_sendBufferUsed
)
{
// Try to refill buffer
pccom_sendBufferPosition
=
0
;
pccom_sendBufferUsed
=
0
;
//if (!sendNextPacket()) return -1;
sendNextPacket
();
}
if
(
pccom_sendBufferPosition
>=
pccom_sendBufferUsed
)
return
-
1
;
// no data
else
{
// Return next byte
uint8_t
data
=
pccom_sendBuffer
[
pccom_sendBufferPosition
];
pccom_sendBufferPosition
++
;
return
data
;
}
}
// ------------------------------ Initialization ------------------------------
void
pccom_init
()
{
serialio_init
();
}
linear_pendulum_2009/avr/pccom.h
0 → 100644
View file @
37d6b571
// pccom.h: Communication interface to PC via serialio_core.
#ifndef __pccom_h
#define __pccom_h
#include
<inttypes.h>
#include
"vel_control.h"
void
pccom_init
();
// Concurrency constraints:
// Receiving and sending are mutually exclusive!
void
pccom_receiveByte
(
char
ch
);
int16_t
pccom_getNextByteToSend
();
// returns -1 for nothing to send
// ------------------------------- Callbacks ----------------------------------
void
serialio_putchar
(
unsigned
char
ch
);
#endif
linear_pendulum_2009/avr/serialio_core.h
0 → 100644
View file @
37d6b571
// serialio_core.h: Serialio protocol without the hardware bindings.
#ifndef __serialio_core_h
#define __serialio_core_h
/*
* Digital in/out and poll commands are sent as one byte:
*
* +-+-+-+-+-+-+-+-+
* |0|0 0| chan | Bit clear
* +-+-+-+-+-+-+-+-+
*
* +-+-+-+-+-+-+-+-+
* |0|0 1| chan | Bit set
* +-+-+-+-+-+-+-+-+
*
* +-+-+-+-+-+-+-+-+
* |0|1 0| chan | Bit get
* +-+-+-+-+-+-+-+-+
*
* +-+-+-+-+-+-+-+-+
* |0|1 1| chan | Channel get
* +-+-+-+-+-+-+-+-+
*
*
* Channels are sent as 2 to 6 bytes, depending on resolution:
*
* +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
* 2 |1| bit8...bit2 | |0|bit| chan |
* +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
*
* +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
* 3 |1|bit15...bit9 | |1| bit8...bit2 | |0|bit| chan |
* +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
*
* ...
*
* +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
* 6 |1|bit31...bit30| |1|bit29...bit23| ... |0|bit| chan |
* +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
*
*
*
* Channel 31 is special, as it serves as the configuration channel. When
* reading from it multiple responses are sent with the following layout
*
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
* | command specific data |cmd|kind |conf chan|
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
*
* kind: 000 == end of configuration
* 001 == digital in
* 010 == digital out
* 011 == analog in
* 100 == analog out
* 101 == counter in
*
*cmd == 0 (Resolution)
*
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
* | | # of bits |0 0|kind |conf chan|
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
*
* # of bits (1..32)
*
*cmd == 1 (Minimum value)
*
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
* | minimum |S| unit|0 1|kind |conf chan|
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
*
* S (sign): 0 == +
* 1 == -
* unit: 000 == V
* 001 == mV
* 010 == uV
* 100 == A
*
*cmd == 2 (Maximum value)
*
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
* | maximum |S| unit|1 0|kind |conf chan|
* +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
*
* S (sign): 0 == +
* 1 == -
* unit: 000 == V
* 001 == mV
* 010 == uV
* 100 == A
*/
// ----------------------------- Interface -----------------------------------
static
void
serialio_init
();
// ---- Receiving ----
typedef
enum
{
serialio_error
,
serialio_more
,
serialio_clearbit
,
serialio_setbit
,
serialio_setchannel
,
serialio_pollbit
,
serialio_pollchannel
}
serialio_rxc_status
;
static
serialio_rxc_status
serialio_RXC
(
unsigned
char
ch
);
// ----- Sending -----
void
serialio_putchar
(
unsigned
char
ch
);
// Callback
static
void
serialio_putbit
(
unsigned
char
channel
,
unsigned
char
value
);
static
void
serialio_putchannel
(
unsigned
char
channel
,
unsigned
long
value
);
// ---- Configuration macros ----
#define CONF_DIG_IN(channel) (0x20 | (channel)&0x1f)
#define CONF_DIG_OUT(channel) (0x40 | (channel)&0x1f)
#define CONF_END() serialio_putchannel(31, 0)
#define CONF_DIGITAL_IN(chan, config) \
serialio_putchannel(31, (0x20|(chan&0x1f)|(config&0xffffff00)))
#define CONF_DIGITAL_OUT(chan, config) \
serialio_putchannel(31, (0x40|(chan&0x1f)|(config&0xffffff00)))
#define CONF_ANALOG_IN(chan, config) \
serialio_putchannel(31, (0x60|(chan&0x1f)|(config&0xffffff00)))
#define CONF_ANALOG_OUT(chan, config) \
serialio_putchannel(31, (0x80|(chan&0x1f)|(config&0xffffff00)))
#define CONF_ENCODER_IN(chan, config) \
serialio_putchannel(31, (0xa0|(chan&0x1f)|(config&0xffffff00)))
#define CONF_RESOLUTION(bits) (((bits)<<10)|0x000)
#define CONF_MIN(value) ((value)|0x100)
#define CONF_MAX(value) ((value)|0x200)
#define CONF_NEGATIVE_VOLT(volt) (((long)(volt)<<14)|0x2000)
#define CONF_POSITIVE_VOLT(volt) ((long)(volt)<<14)
#define CONF_NEGATIVE_MILLIVOLT(millivolt) (((long)(millivolt)<<14)|0x2400)
#define CONF_POSITIVE_MILLIVOLT(millivolt) ((long)(millivolt)<<14|0x400)
#define CONF_POSITIVE_AMPERE(ampere) (((long)(ampere)<<14)|0x1000)
// --------------------------- Implementation ---------------------------------
static
volatile
unsigned
long
serialio_value
;
static
volatile
unsigned
char
serialio_channel
,
serialio_length
;
static
void
serialio_putbit
(
unsigned
char
channel
,
unsigned
char
value
)
{
if
(
value
)
{
serialio_putchar
(
0x20
|
(
channel
&
0x1f
));
}
else
{
serialio_putchar
(
0x00
|
(
channel
&
0x1f
));
}
}
static
void
serialio_putchannel
(
unsigned
char
channel
,
unsigned
long
value
)
{
if
(
value
>=
(
1L
<<
30
))
{
serialio_putchar
(
0x80
|
((
value
>>
30
)
&
0x03
));
}
if
(
value
>=
(
1L
<<
23
))
{
serialio_putchar
(
0x80
|
((
value
>>
23
)
&
0x7f
));
}
if
(
value
>=
(
1L
<<
16
))
{
serialio_putchar
(
0x80
|
((
value
>>
16
)
&
0x7f
));
}
if
(
value
>=
(
1L
<<
9
))
{
serialio_putchar
(
0x80
|
((
value
>>
9
)
&
0x7f
));
}
//serialio_putchar(0x80 | ((value >> 9) & 0x7f)); // DEBUG
serialio_putchar
(
0x80
|
((
value
>>
2
)
&
0x7f
));
serialio_putchar
(((
value
<<
5
)
&
0x60
)
|
(
channel
&
0x1f
));
}
static
void
serialio_init
()
{
serialio_value
=
0
;
serialio_channel
=
255
;
serialio_length
=
0
;
}
static
serialio_rxc_status
serialio_RXC
(
unsigned
char
ch
)
{
unsigned
char
result
=
serialio_error
;
if
(
serialio_length
==
0
)
{
serialio_value
=
0
;
}
serialio_length
++
;
if
((
ch
&
0x80
)
==
0x80
)
{
// Collect yet another byte for later processing
serialio_value
=
(
serialio_value
<<
7
)
|
(
ch
&
0x7f
);
result
=
serialio_more
;
}
else
{
serialio_value
=
(
serialio_value
<<
2
)
|
((
ch
&
0x60
)
>>
5
);
serialio_channel
=
ch
&
0x1f
;
if
(
serialio_length
==
1
)
{
switch
(
serialio_value
&
0x03
)
{
// Digital output buffer (ULN2803A) is inverting
case
0
:
{
result
=
serialio_clearbit
;
}
break
;
case
1
:
{
result
=
serialio_setbit
;
}
break
;
case
2
:
{
result
=
serialio_pollbit
;
}
break
;
case
3
:
{
result
=
serialio_pollchannel
;
}
break
;
}
}
else
{
result
=
serialio_setchannel
;
}
serialio_length
=
0
;
}
return
result
;
}
#endif
linear_pendulum_2009/avr/vel_control.c
View file @
37d6b571
...
...
@@ -5,11 +5,16 @@
#include
<inttypes.h>
#include
"pccom.h"
#include
"vel_control.h"
// reference variables
volatile
int32_t
ref
=
0
;
// 11 frac bits
volatile
int16_t
refFlag
=
0
;
volatile
int16_t
deltaRef
=
2
;
volatile
int16_t
deltaRef
=
1
;
volatile
int16_t
refCount
=
0
;
volatile
int32_t
refTest
=
0
;
// velocity control variables
...
...
@@ -61,6 +66,17 @@ static inline void sendData() {
}
// return position (in tics)
int32_t
getPosition
()
{
return
pos
;
}
// return velocity (in mm/s)
int32_t
getVelocity
()
{
return
velEst
;
}
/* Routine used to set the red LED */
void
setLED
(
uint8_t
on
)
...
...
@@ -69,6 +85,12 @@ void setLED(uint8_t on)
else
PORTB
|=
0x80
;
//Turn off
}
// Set new reference value
void
setRef
(
int32_t
newRef
)
{
ref
=
newRef
;
}
/* Routine used to initialize the positional encoders */
void
initPos
()
{
...
...
@@ -157,6 +179,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
u
=
-
128
;
}
//u = 2;
/*
// reference calculations
refCount++;
...
...
@@ -177,7 +202,7 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
deltaRef = -deltaRef;
}
}
*/
/*
if (refCount == 1000) {
...
...
@@ -186,7 +211,7 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
*/
pos
=
pos
*
(
1
-
brake
);
ref
=
ref
*
(
1
-
brake
);
// TWI-communication
...
...
@@ -211,29 +236,29 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
// 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
));
// ------- 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
();
}
}
PORTC
&=
~
0x10
;
...
...
@@ -267,7 +292,7 @@ int main()
//Serial communication
outp
(
0x00
,
UCSRA
);
// USART:
outp
(
0x
9
8
,
UCSRB
);
// USART:
RxIntEnable|
RxEnable|TxEnable
outp
(
0x
1
8
,
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
...
...
@@ -287,6 +312,8 @@ int main()
// initialize position measurements
initPos
();
pccom_init
();
//Enable interrupts
sei
();
...
...
linear_pendulum_2009/avr/vel_control.h
0 → 100644
View file @
37d6b571
#ifndef __vel_control_h
#define __vel_control_h
void
setRef
(
int32_t
newRef
);
int32_t
getPosition
();
int32_t
getVelocity
();
#endif
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