diff --git a/common/avr/Makefile.common b/lib/avr/Makefile.common similarity index 96% rename from common/avr/Makefile.common rename to lib/avr/Makefile.common index 8cdb9da4b0a194a9c990a1c4c7abb66ef4de8cdb..aa15db4feaed0c2848036428ab7c1b623360ad87 100644 --- a/common/avr/Makefile.common +++ b/lib/avr/Makefile.common @@ -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) diff --git a/common/avr/README b/lib/avr/README similarity index 100% rename from common/avr/README rename to lib/avr/README diff --git a/common/avr/serialio.h b/lib/avr/serialio.h similarity index 93% rename from common/avr/serialio.h rename to lib/avr/serialio.h index 0710deaf81a1caf0c631872f680da6034f9e9c5a..683a8015f34eceb488d0da28b44219de98d85ee3 100644 --- a/common/avr/serialio.h +++ b/lib/avr/serialio.h @@ -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; } diff --git a/linear_pendulum_2009/avr/Makefile b/linear_pendulum_2009/avr/Makefile index 0aaf4aea718ab3947374c02e4cfe07c18b048bce..dc03524ae66cd678dffeae62954e6f98c3098d85 100644 --- a/linear_pendulum_2009/avr/Makefile +++ b/linear_pendulum_2009/avr/Makefile @@ -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 diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c deleted file mode 100644 index f85bd4bc33a6ff0bdbf468a74aa2f20cffccbed7..0000000000000000000000000000000000000000 --- a/linear_pendulum_2009/avr/pccom.c +++ /dev/null @@ -1,206 +0,0 @@ -// 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(); -} diff --git a/linear_pendulum_2009/avr/pccom.h b/linear_pendulum_2009/avr/pccom.h deleted file mode 100644 index 14c4c75d7da795a47f3e884364c2ab03e3e1353e..0000000000000000000000000000000000000000 --- a/linear_pendulum_2009/avr/pccom.h +++ /dev/null @@ -1,21 +0,0 @@ -// 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 diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c index 5df3ca8c82dc6ca1ebb66f917c7cdfa1c3ab1681..69a20cd690a6a52305c13e8edfa6124ceb8b4f90 100644 --- a/linear_pendulum_2009/avr/vel_control.c +++ b/linear_pendulum_2009/avr/vel_control.c @@ -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(); + } } }