Skip to content

GitLab

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

Moved serial communication to iterrupt routine.

parent b2bcd0e0
Hide whitespace changes
Inline Side-by-side
Showing with 156 additions and 258 deletions
common/avr/Makefile.common → lib/avr/Makefile.common
View file @ bac2e03c
......@@ -36,7 +36,7 @@ all: $(TARGETS:%=%.LINK)
else
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
ARCH=$($(TARGET).ARCH)
......
common/avr/serialio.h → lib/avr/serialio.h
View file @ bac2e03c
......@@ -85,17 +85,19 @@
* 100 == A
*/
#include <avr/io.h>
static volatile unsigned long serialio_value;
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) {};
out(UDR, ch);
while ((UCSRA & 0x20) == 0) {};
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) {
serialio_putchar(0x20 | (channel & 0x1f));
......@@ -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<<23)) { serialio_putchar(0x80 | ((value >> 23) & 0x7f)); }
......@@ -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_AMPERE(ampere) (((long)(ampere)<<14)|0x1000)
static void serialio_init()
static inline void serialio_init()
{
serialio_value = 0;
serialio_channel = 255;
......@@ -151,7 +156,7 @@ typedef enum {
} 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;
if (serialio_length == 0) { serialio_value = 0; }
......
linear_pendulum_2009/avr/Makefile
View file @ bac2e03c
......@@ -10,7 +10,7 @@ vel_control.ARCH=avr
vel_control.CHIP=atmega16
# 14.7456 MHz crystal, brown out
vel_control.FUSE=--wr_fuse_l=0x1f --wr_fuse_h=0xd9 --wr_fuse_e=0xff
vel_control.C=vel_control pccom
vel_control.H=pccom serialio_core
vel_control.C=vel_control
vel_control.H=serialio_core
include ../../common/avr/Makefile.common
include ../../lib/avr/Makefile.common
linear_pendulum_2009/avr/pccom.c deleted 100644 → 0
View file @ b2bcd0e0
// 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();
}
linear_pendulum_2009/avr/pccom.h deleted 100644 → 0
View file @ b2bcd0e0
// 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/vel_control.c
View file @ bac2e03c
......@@ -12,14 +12,43 @@
#include <inttypes.h>
#include <compat/deprecated.h>
#include "pccom.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
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 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
// velocity control variables
......@@ -46,7 +75,6 @@ volatile int8_t fr_comp = (10<<3);
// position variables
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 oldPos = 0; // previous pos used for velocity estimation
volatile int8_t newX; // encoder signal
......@@ -67,10 +95,11 @@ volatile int16_t angleOffset = 0;
/* return position (in tics) */
int32_t getPosition() {
int16_t result = 0;
cli();
posTemp = pos;
result = pos;
sei();
return ((int32_t) posTemp);
return (result);
}
......@@ -84,7 +113,8 @@ int32_t getVelocity() {
int16_t getAngle() {
low = inp(ADCL);
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() {
/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
aRef = newAccRef;
acc_ref = newAccRef;
nbrSamples = 0;
}
/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
ref = newVelRef;
vel_ref = newVelRef;
}
/* Routine used to initialize the positional encoders */
......@@ -115,11 +145,10 @@ void initPos()
/* Timer 2, Encoder counter, 73 kHz updates position variable */
SIGNAL(TIMER2_COMP_vect) {
// Update position from encoder
newX = ENCODERX;
newY = ENCODERY;
if((newX != oldX) || (newY != oldY)) { //Check if any value changed
if((newX != oldX) || (newY != oldY)) { //Check if any value changed
/*
sum = (oldX<<2)+oldY+newX+(newY>>2);
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
......@@ -141,10 +170,51 @@ SIGNAL(TIMER2_COMP_vect) {
oldX = newX;
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 */
SIGNAL(TIMER0_COMP_vect) {
......@@ -174,12 +244,11 @@ SIGNAL(TIMER0_COMP_vect) {
TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts
}
#endif
/* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) {
posCtrl = pos; // store pos to use in this loop
sei(); // to enable interupts from encoder counter and communication
......@@ -192,11 +261,11 @@ SIGNAL(TIMER1_COMPA_vect) {
cli();
velEstSend = velEst;
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) {
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();
// control error
......@@ -257,7 +326,6 @@ SIGNAL(TIMER1_COMPA_vect) {
// stop transmission
outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR);
}
......@@ -266,7 +334,7 @@ int main()
cli();
//Port directions
//outp(0x80,DDRB); // Led output
//outp(0xf0,DDRB); // Led output
outp(0x10,DDRC); // timer calculation port
PORTD = 0x40; // pull up on reset switch
......@@ -274,6 +342,7 @@ int main()
/* Timer section */
// Enable timer0, timer1, timer2 compare match interrupts
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 */
outp(_BV(WGM21)|_BV(CS20),TCCR2);
......@@ -307,6 +376,12 @@ int main()
outp(0x86, UCSRC); // USART: 8bit, no parity
outp(0x00, UBRRH); // 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; // USART: 115200 @ 14.7456MHz
/* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
......@@ -324,13 +399,15 @@ int main()
initPos();
// initialize pc-communication
pccom_init();
serialio_init();
pccom_poll = 0;
//Enable interrupts
sei();
// loop
while (1) {
unsigned char to_poll;
// reset position, velocity estimate and integral part of ctrl if reset button pushed
if (!(PIND & 0x40)) {
cli();
......@@ -343,6 +420,49 @@ int main()
I = 0; // reset integral part of controller
u = 0; // reset ctrl signal
sei();
}
}
cli();
to_poll = pccom_poll;
pccom_poll = 0;
sei();
if (to_poll & POLL_AXIS1_POSITION) {
serialio_putchannel(0, getPosition()+(1L<<15));
}
if (to_poll & POLL_AXIS1_VELOCITY) {
serialio_putchannel(1, getVelocity()+(1L<<17));
}
if (to_poll & POLL_PEND_ANGLE) {
serialio_putchannel(2, getAngle()+(1L<<10));
}
if (to_poll & POLL_CURRENT_REFERENCE) {
serialio_putchannel(3, getCurrentRef()+(1L<<7));
}
if (to_poll & POLL_CONFIG) {
CONF_ANALOG_IN(0, CONF_RESOLUTION(16)); // Position (now reference)
CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1)));
CONF_ANALOG_IN(1, CONF_RESOLUTION(18)); // Velocity estimate
CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_VOLT(1)));
CONF_ANALOG_IN(2, CONF_RESOLUTION(11)); // Pend angle measurement
CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_VOLT(1)));
CONF_ANALOG_IN(3, CONF_RESOLUTION(8)); // Current reference
CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_VOLT(1)));
CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); // Reference to vel-ctrl
CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1)));
CONF_ANALOG_OUT(1, CONF_RESOLUTION(16)); // Reference to acc-ctrl
CONF_ANALOG_OUT(1, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
CONF_ANALOG_OUT(1, CONF_MAX(CONF_POSITIVE_VOLT(1)));
CONF_END();
}
}
}
Markdown is supported
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