diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c
new file mode 100644
index 0000000000000000000000000000000000000000..861698bdbde6829a66a76e32b7a2281fef544d9c
--- /dev/null
+++ b/linear_pendulum_2009/avr/pccom.c
@@ -0,0 +1,188 @@
+// 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();
+}
diff --git a/linear_pendulum_2009/avr/pccom.h b/linear_pendulum_2009/avr/pccom.h
new file mode 100644
index 0000000000000000000000000000000000000000..14c4c75d7da795a47f3e884364c2ab03e3e1353e
--- /dev/null
+++ b/linear_pendulum_2009/avr/pccom.h
@@ -0,0 +1,21 @@
+// 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/serialio_core.h b/linear_pendulum_2009/avr/serialio_core.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ca09ae04477744fc4cec9bb34c4def7e70ca5cf
--- /dev/null
+++ b/linear_pendulum_2009/avr/serialio_core.h
@@ -0,0 +1,197 @@
+// 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
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 17e6e1f192e4e2a58fb7f1188771ddb5f7382f4e..598efe7b5144fce399dfcfeebf31492d2ae36c59 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -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();
diff --git a/linear_pendulum_2009/avr/vel_control.h b/linear_pendulum_2009/avr/vel_control.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ffacdd4e424206f19e9fa35df680db9b5262bd9
--- /dev/null
+++ b/linear_pendulum_2009/avr/vel_control.h
@@ -0,0 +1,9 @@
+#ifndef __vel_control_h
+#define __vel_control_h
+
+
+void setRef(int32_t newRef);
+int32_t getPosition();
+int32_t getVelocity();
+
+#endif