Commit bac2e03c authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Moved serial communication to iterrupt routine.

parent b2bcd0e0
...@@ -36,7 +36,7 @@ all: $(TARGETS:%=%.LINK) ...@@ -36,7 +36,7 @@ all: $(TARGETS:%=%.LINK)
else else
avr-CC=avr-gcc avr-CC=avr-gcc
avr-CCFLAGS=-mmcu=$(CHIP) -g -Wall -Werror -O3 -I. -I../lib avr-CCFLAGS=-mmcu=$(CHIP) -g -Wall -Werror -O3 -I. -I../../lib/avr
avr-OBJCOPY=avr-objcopy avr-OBJCOPY=avr-objcopy
ARCH=$($(TARGET).ARCH) ARCH=$($(TARGET).ARCH)
......
...@@ -85,17 +85,19 @@ ...@@ -85,17 +85,19 @@
* 100 == A * 100 == A
*/ */
#include <avr/io.h>
static volatile unsigned long serialio_value; static volatile unsigned long serialio_value;
static volatile unsigned char serialio_channel, serialio_length; static volatile unsigned char serialio_channel, serialio_length;
static void serialio_putchar(unsigned char ch) static inline void serialio_putchar(unsigned char ch)
{ {
while ((in(UCSRA) & 0x20) == 0) {}; while ((UCSRA & 0x20) == 0) {};
out(UDR, ch); UDR = ch;
} }
static void serialio_putbit(unsigned char channel, unsigned char value) static inline void serialio_putbit(unsigned char channel, unsigned char value)
{ {
if (value) { if (value) {
serialio_putchar(0x20 | (channel & 0x1f)); serialio_putchar(0x20 | (channel & 0x1f));
...@@ -104,7 +106,10 @@ static void serialio_putbit(unsigned char channel, unsigned char value) ...@@ -104,7 +106,10 @@ static void serialio_putbit(unsigned char channel, unsigned char value)
} }
} }
static void serialio_putchannel(unsigned char channel, unsigned long value)
static inline void serialio_putchannel(unsigned char channel,
unsigned long value)
{ {
if (value >= (1L<<30)) { serialio_putchar(0x80 | ((value >> 30) & 0x03)); } if (value >= (1L<<30)) { serialio_putchar(0x80 | ((value >> 30) & 0x03)); }
if (value >= (1L<<23)) { serialio_putchar(0x80 | ((value >> 23) & 0x7f)); } if (value >= (1L<<23)) { serialio_putchar(0x80 | ((value >> 23) & 0x7f)); }
...@@ -138,7 +143,7 @@ static void serialio_putchannel(unsigned char channel, unsigned long value) ...@@ -138,7 +143,7 @@ static void serialio_putchannel(unsigned char channel, unsigned long value)
#define CONF_POSITIVE_MILLIVOLT(millivolt) ((long)(millivolt)<<14|0x400) #define CONF_POSITIVE_MILLIVOLT(millivolt) ((long)(millivolt)<<14|0x400)
#define CONF_POSITIVE_AMPERE(ampere) (((long)(ampere)<<14)|0x1000) #define CONF_POSITIVE_AMPERE(ampere) (((long)(ampere)<<14)|0x1000)
static void serialio_init() static inline void serialio_init()
{ {
serialio_value = 0; serialio_value = 0;
serialio_channel = 255; serialio_channel = 255;
...@@ -151,7 +156,7 @@ typedef enum { ...@@ -151,7 +156,7 @@ typedef enum {
} serialio_rxc_status; } serialio_rxc_status;
static serialio_rxc_status serialio_RXC(unsigned char ch) { static inline serialio_rxc_status serialio_RXC(unsigned char ch) {
unsigned char result = serialio_error; unsigned char result = serialio_error;
if (serialio_length == 0) { serialio_value = 0; } if (serialio_length == 0) { serialio_value = 0; }
......
...@@ -10,7 +10,7 @@ vel_control.ARCH=avr ...@@ -10,7 +10,7 @@ vel_control.ARCH=avr
vel_control.CHIP=atmega16 vel_control.CHIP=atmega16
# 14.7456 MHz crystal, brown out # 14.7456 MHz crystal, brown out
vel_control.FUSE=--wr_fuse_l=0x1f --wr_fuse_h=0xd9 --wr_fuse_e=0xff vel_control.FUSE=--wr_fuse_l=0x1f --wr_fuse_h=0xd9 --wr_fuse_e=0xff
vel_control.C=vel_control pccom vel_control.C=vel_control
vel_control.H=pccom serialio_core vel_control.H=serialio_core
include ../../common/avr/Makefile.common include ../../lib/avr/Makefile.common
// 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_CURRENT_REFERENCE 0x0008
//#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 3: { addPoll(POLL_CURRENT_REFERENCE); } break;
case 31: { addPoll(POLL_CONFIG); } break;
}
} break;
case serialio_setchannel: {
switch (serialio_channel) {
case 0: {
setVelRef(serialio_value - (1L<<12));
} break;
case 1: {
setAccRef(serialio_value - (1L<<15));
} 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_IN(2, CONF_RESOLUTION(11)); break; // Pend angle measurement
case 7: CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
case 8: CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
case 9: CONF_ANALOG_IN(3, CONF_RESOLUTION(8)); break; // Current reference
case 10: CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
case 11: CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
case 12: CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); break; // Reference to vel-ctrl
case 13: CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
case 14: CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
case 15: CONF_ANALOG_OUT(1, CONF_RESOLUTION(16)); break; // Reference to acc-ctrl
case 16: CONF_ANALOG_OUT(1, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
case 17: CONF_ANALOG_OUT(1, 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;
#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_PEND_ANGLE) {
toPoll &= ~POLL_PEND_ANGLE;
serialio_putchannel(2, getAngle()+(1L<<10));
}
else if (toPoll & POLL_CURRENT_REFERENCE) {
toPoll &= ~POLL_CURRENT_REFERENCE;
serialio_putchannel(3, getCurrentRef()+(1L<<7));
}
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();
}
// 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
...@@ -12,14 +12,43 @@ ...@@ -12,14 +12,43 @@
#include <inttypes.h> #include <inttypes.h>
#include <compat/deprecated.h> #include <compat/deprecated.h>
#include "pccom.h"
#include "vel_control.h" #include "vel_control.h"
#include "serialio.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 0x01
#define POLL_AXIS1_VELOCITY 0x02
#define POLL_PEND_ANGLE 0x04
#define POLL_CURRENT_REFERENCE 0x08
//#define POLL_AXIS1_RESET 0x0008
#define POLL_CONFIG 0x80
static volatile uint8_t pccom_poll=0;
// reference variables // reference variables
volatile int32_t ref = 0; // 11 frac bits (variable in mm/s, max 2^12) volatile int32_t vel_ref = 0; // 11 frac bits (variable in mm/s, max 2^12)
volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink) volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
volatile int32_t aRef = 0; // 14 frac bits (variable in mm/s, max 2^15) volatile int32_t acc_ref = 0; // 14 frac bits (variable in mm/s, max 2^15)
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
// velocity control variables // velocity control variables
...@@ -46,7 +75,6 @@ volatile int8_t fr_comp = (10<<3); ...@@ -46,7 +75,6 @@ volatile int8_t fr_comp = (10<<3);
// position variables // position variables
volatile int16_t pos = 0; // position in tics volatile int16_t pos = 0; // position in tics
volatile int16_t posTemp = 0; // position sent to simulink
volatile int16_t posCtrl = 0; // position used in ctrl-loop volatile int16_t posCtrl = 0; // position used in ctrl-loop
volatile int16_t oldPos = 0; // previous pos used for velocity estimation volatile int16_t oldPos = 0; // previous pos used for velocity estimation
volatile int8_t newX; // encoder signal volatile int8_t newX; // encoder signal
...@@ -67,10 +95,11 @@ volatile int16_t angleOffset = 0; ...@@ -67,10 +95,11 @@ volatile int16_t angleOffset = 0;
/* return position (in tics) */ /* return position (in tics) */
int32_t getPosition() { int32_t getPosition() {
int16_t result = 0;
cli(); cli();
posTemp = pos; result = pos;
sei(); sei();
return ((int32_t) posTemp); return (result);
} }
...@@ -84,7 +113,8 @@ int32_t getVelocity() { ...@@ -84,7 +113,8 @@ int32_t getVelocity() {
int16_t getAngle() { int16_t getAngle() {
low = inp(ADCL); low = inp(ADCL);
high = inp(ADCH); high = inp(ADCH);
return (((int16_t) ((high<<8) | low) - 512)-angleOffset); int16_t result = ADC - 512 -angleOffset;
return result;
} }
...@@ -96,14 +126,14 @@ int32_t getCurrentRef() { ...@@ -96,14 +126,14 @@ int32_t getCurrentRef() {
/* Set new acceleration reference value */ /* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) { void setAccRef(int32_t newAccRef) {
aRef = newAccRef; acc_ref = newAccRef;
nbrSamples = 0; nbrSamples = 0;
} }
/* Set new velocity reference value */ /* Set new velocity reference value */
void setVelRef(int32_t newVelRef) { void setVelRef(int32_t newVelRef) {
ref = newVelRef; vel_ref = newVelRef;
} }
/* Routine used to initialize the positional encoders */ /* Routine used to initialize the positional encoders */
...@@ -115,7 +145,6 @@ void initPos() ...@@ -115,7 +145,6 @@ void initPos()
/* Timer 2, Encoder counter, 73 kHz updates position variable */ /* Timer 2, Encoder counter, 73 kHz updates position variable */
SIGNAL(TIMER2_COMP_vect) { SIGNAL(TIMER2_COMP_vect) {
// Update position from encoder // Update position from encoder
newX = ENCODERX; newX = ENCODERX;
newY = ENCODERY; newY = ENCODERY;
...@@ -141,10 +170,51 @@ SIGNAL(TIMER2_COMP_vect) { ...@@ -141,10 +170,51 @@ SIGNAL(TIMER2_COMP_vect) {
oldX = newX; oldX = newX;
oldY = newY; oldY = newY;
} }
} }
SIGNAL(USART_RXC_vect) {
char ch = UDR;
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: pccom_poll |= POLL_AXIS1_POSITION; break;
case 1: pccom_poll |= POLL_AXIS1_VELOCITY; break;
case 2: pccom_poll |= POLL_PEND_ANGLE; break;
case 3: pccom_poll |= POLL_CURRENT_REFERENCE; break;
case 31: pccom_poll |= POLL_CONFIG; break;
}
} break;
case serialio_setchannel: {
switch (serialio_channel) {
case 0: {
setVelRef(serialio_value - (1L<<12));
} break;
case 1: {
setAccRef(serialio_value - (1L<<15));
} break;
}
} break;
case serialio_more: {
} break;
case serialio_error: {
} break;
}
}
#if 0
/* Timer 0, serial communication with simulink */ /* Timer 0, serial communication with simulink */
SIGNAL(TIMER0_COMP_vect) { SIGNAL(TIMER0_COMP_vect) {
...@@ -174,12 +244,11 @@ SIGNAL(TIMER0_COMP_vect) { ...@@ -174,12 +244,11 @@ SIGNAL(TIMER0_COMP_vect) {
TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts
} }
#endif
/* Timer 0, control loop , 1 kHz */ /* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) { SIGNAL(TIMER1_COMPA_vect) {
posCtrl = pos; // store pos to use in this loop posCtrl = pos; // store pos to use in this loop
sei(); // to enable interupts from encoder counter and communication sei(); // to enable interupts from encoder counter and communication
...@@ -192,11 +261,11 @@ SIGNAL(TIMER1_COMPA_vect) { ...@@ -192,11 +261,11 @@ SIGNAL(TIMER1_COMPA_vect) {
cli(); cli();
velEstSend = velEst; velEstSend = velEst;
nbrSamples++; nbrSamples++;
refCtrl = ref+((aRef*nbrSamples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h) refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
if (nbrSamples > 500) { if (nbrSamples > 500) {
refCtrl = 0; // for safety reasons if doesn't send anything in 0.5 s refCtrl = 0; // for safety reasons if doesn't send anything in 0.5 s
} }
// ref = ref*(1-brake); // emergency stop // vel_ref = vel_ref*(1-brake); // emergency stop
sei(); sei();
// control error // control error
...@@ -257,7 +326,6 @@ SIGNAL(TIMER1_COMPA_vect) { ...@@ -257,7 +326,6 @@ SIGNAL(TIMER1_COMPA_vect) {
// stop transmission // stop transmission
outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR); outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR);
} }
...@@ -266,7 +334,7 @@ int main() ...@@ -266,7 +334,7 @@ int main()
cli(); cli();
//Port directions //Port directions
//outp(0x80,DDRB); // Led output //outp(0xf0,DDRB); // Led output
outp(0x10,DDRC); // timer calculation port outp(0x10,DDRC); // timer calculation port
PORTD = 0x40; // pull up on reset switch PORTD = 0x40; // pull up on reset switch
...@@ -274,6 +342,7 @@ int main() ...@@ -274,6 +342,7 @@ int main()
/* Timer section */ /* Timer section */
// Enable timer0, timer1, timer2 compare match interrupts // Enable timer0, timer1, timer2 compare match interrupts
outp(_BV(OCIE0)|_BV(OCIE1A)|_BV(OCIE2),TIMSK); outp(_BV(OCIE0)|_BV(OCIE1A)|_BV(OCIE2),TIMSK);
outp(_BV(OCIE1A)|_BV(OCIE2),TIMSK);
/* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */ /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
outp(_BV(WGM21)|_BV(CS20),TCCR2); outp(_BV(WGM21)|_BV(CS20),TCCR2);
...@@ -308,6 +377,12 @@ int main() ...@@ -308,6 +377,12 @@ int main()
outp(0x00, UBRRH); // USART: 115200 @ 14.7456MHz outp(0x00, UBRRH); // USART: 115200 @ 14.7456MHz
outp(7,UBRRL); // USART: 115200 @ 14.7456MHz outp(7,UBRRL); // USART: 115200 @ 14.7456MHz
UCSRA = 0x00; // USART:
UCSRB = 0x98; // USART: RxIntEnable|RxEnable|TxEnable
UCSRC = 0x86; // USART: 8bit, no parity, 9600
UBRRH = 0x0; // USART: 115200 @ 14.7456MHz
UBRRL = 7;