Skip to content
GitLab
Commit 37d6b571 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Working velocity control with simulink communication

parent 4cf532d5
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(0x98, UCSRB); // USART: RxIntEnable|RxEnable|TxEnable
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
......@@ -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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment