Commit 8dffe83f authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Merge parts of L001-AVRCurrentControl

parents f5b7fc8c 333c8293
PROJECT=current_control
CHIP=atmega16
ARCH=avr
AVRSOURCE=
AVRAUTO=compiled/$(ARCH)/
AVRARCH=compiled/$(ARCH)/
LIB_TARGET=$(AVRARCH)lib$(TARGET).a
CC=$(ARCH)-gcc
CXX=$(ARCH)-gcc
CCFLAGS=-mmcu=$(CHIP) -O5 -I. -I$(AVRAUTO)
AS=$(ARCH)-as
ASFLAGS=-mmcu=$(CHIP)
AR=$(ARCH)-ar
LD=$(ARCH)-ld
OBJDUMP=$(ARCH)-objdump
LDFLAGS= -T $(CHIP).link
all: $(AVRARCH)$(PROJECT)
$(LIB_TARGET)(%.o): $(AVRSOURCE)%.c Makefile
$(CC) $(CCFLAGS) -c -o $(AVRARCH)$(%) -I$(AVRSOURCE) -I$(AVRAUTO) $(<)
$(AR) r $(@) $(AVRARCH)$(%)
rm $(AVRARCH)$(%)
$(LIB_TARGET)(%.o): $(AVRSOURCE)%.cc Makefile
$(CXX) $(CCFLAGS) -c -o $(AVRARCH)$(%) -I$(AVRSOURCE) -I$(AVRAUTO) $(<)
$(AR) r $(@) $(AVRARCH)$(%)
rm $(AVRARCH)$(%)
$(LIB_TARGET)(%.o): $(AVRSOURCE)%.s Makefile
$(AS) $(ASFLAGS) -o $(AVRARCH)$(%) $(AVRSOURCE)$(*).s
$(AR) r $(@) $(AVRARCH)$(%)
rm $(AVRARCH)$(%)
$(AVRARCH)%.o: $(AVRSOURCE)%.s Makefile
$(AS) $(ASFLAGS) -o $@ $(AVRSOURCE)$(*).s
$(AVRARCH)%.o: $(AVRSOURCE)%.c Makefile
$(CC) $(CCFLAGS) -c -o $@ -I. -I$(AVRAUTO) $<
$(AVRARCH)%: $(AVRARCH)%.o Makefile
$(CC) $(CCFLAGS) -o $@ $< -lm
$(AVRARCH)%.sr: all $(AVRARCH)%
avr-objcopy -O srec $(AVRARCH)/$(*) $@
%.sr: $(AVRARCH)%.sr
@/bin/true
%.load: compiled/avr/%.sr
# /work/andersb/atmel/uisp/uisp-0.2b/src/uisp \
# -dno-poll -dstk200 --erase --upload if=$(AVRARCH)/$(*).sr
uisp \
-dprog=stk200 \
--erase \
--upload if=compiled/avr/$*.sr
%.load.stk500: compiled/avr/%.sr
uisp \
-dprog=stk500 \
--erase \
--upload if=compiled/avr/$*.sr
%.dump: all $(AVRARCH)/%
$(OBJDUMP) -D $(AVRARCH)/$(*)
$(AVRARCH)fuse.sr: fuse.s
avr-gcc -o $(AVRARCH)fuse.o -c fuse.s
avr-objcopy -O srec $(AVRARCH)fuse.o $(AVRARCH)fuse.sr
fuse.load: $(AVRARCH)fuse.sr
uisp \
-dprog=stk200 \
--erase \
--segment=fuse \
--upload if=compiled/avr/fuse.sr
load.stk500: $(PROJECT).load.stk500
\ No newline at end of file
avr-gcc -mmcu=atmega16 -g -Wall -o current_control current_control.c
avr-objcopy -Osrec current_control current_control.sr
uisp -dprog=stk200 --erase --upload if=current_control.sr
\ No newline at end of file
avr-gcc -mmcu=atmega16 -O -g -Wall -o vel_control.o vel_control.c pccom.c
avr-objcopy -Osrec vel_control.o vel_control.sr
uisp -dprog=stk200 --erase --upload if=vel_control.sr
\ No newline at end of file
/*
***************************************************************
Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
for LTH - reglerteknik
***************************************************************
*/
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>
// control variables
volatile int16_t ref=0; // reference value (from velocity controller)
volatile int16_t y; // measurement, 9 frac bits
volatile uint16_t low, high; // when reading AD-conversion
volatile int16_t e = 0; // control error, 9 frac bits
volatile int16_t v = 0; // temporary ctrl signal, 9 frac bits
volatile int16_t vSat = 0; // saturated temporary ctrl signal, 9 frac bits
volatile int16_t I = 0; // integral part of ctrl, 13 frac bits
volatile int16_t u = 0; // ctrl signal = pwm high time (8 bits)
volatile int16_t K = 111; // 7 frac bits
volatile int16_t Ke = 38; // 7 frac bits, K*h/Ti
volatile int8_t intCond = 0; // flag for conditional integration
#define V_MAX 508 //max/min for saturation
#define V_MIN -508
// twi variable
volatile int16_t status;
// logging variables
/*
#define log_len 100
volatile int16_t ctrl_log[log_len];
volatile int16_t error_log[log_len];
volatile int32_t I_log[log_len];
volatile int16_t skipSamples = 1000;
volatile int16_t countSamples = 0;
volatile int16_t jj=0;
volatile int8_t stop = 0;
*/
/* Routine used to transmit bytes on the serial port */
/*
static void putchar(unsigned char ch)
{
while ((inp(UCSRA) & 0x20) == 0) {};
outp(ch, UDR);
while ((inp(UCSRA) & 0x20) == 0) {};
}
*/
/* Send logged data over Serial connection */
/*
static inline void sendData() {
int16_t ii = 0;
while (ii < log_len) {
putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8));
putchar((unsigned char) (ctrl_log[ii]&0x00ff));
putchar((unsigned char) ((error_log[ii]&0xff00)>>8));
putchar((unsigned char) (error_log[ii]&0x00ff));
putchar((unsigned char) ((I_log[ii]&0xff000000)>>24));
putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16));
putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
putchar((unsigned char) (I_log[ii]&0x000000ff));
ii++;
}
}
*/
/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL(SIG_OUTPUT_COMPARE2) {
// Start AD conversion
ADCSRA |= BV(ADSC);
// Read previous AD-conversion result
low = inp(ADCL);
high = inp(ADCH);
y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits
// control error
e = ref-y; // e 9 frac bits
// temporary ctrl-signal
v = (((K*e+64)>>7)+((I+8)>>4));
// variable that decides if I-part should be updated
intCond = 1;
// saturation and update integral part of ctrl with antiwindup
if (v > V_MAX) {
vSat = V_MAX;
if (e > 0)
intCond = 0;
} else if (v < V_MIN) {
vSat = V_MIN;
if (e < 0)
intCond = 0;
} else {
vSat = v;
}
if (intCond)
I = I + (((Ke*e)+(1<<2))>>3);
// ctrl signal, 7 bits + direction
u = ((vSat+2)>>2); //7 frac bits to pwm
// set pwm switching time
if (u < 0) {
PORTC |= 0x80; // set direction of motion
OCR1BL = ((unsigned char) (-u)); // set length of pwm-high
} else {
PORTC &= 0x7f; // set direction of motion
OCR1BL = ((unsigned char) (u)); // set length of pwm-high
}
// TWI-communication, recieve reference from velocity controller
if ((BV(TWINT)&inp(TWCR))) {
status = (inp(TWSR)&0xf8);
// status 0x80 means data recieved
if (status == 0x80) {
ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference
ref = (ref<<2); // shift up 2 steps for 10 bits reference in loop
}
else {
}
outp(BV(TWINT)|BV(TWEN)|BV(TWEA),TWCR);
}
// For logging purposes
/*
countSamples++;
if (countSamples == skipSamples) {
ctrl_log[jj] = u;
I_log[jj] = I;
error_log[jj] = e;
jj++;
countSamples = 0;
// Stop after a while
if ((jj == (log_len-1)) & !stop) {
outp(0x7f,OCR1BL);
stop = 1;
sendData();
}
}
*/
}
int main()
{
// clear interrupts (might not be needed)
cli();
//Port directions
outp(0x08,PORTC); // pull up on overtemperature signals
outp(0xa0,DDRC); // output on direction and brake
outp(0x10,DDRD); // output on pwm-signal
outp(0x40,DDRB); // temp pwm output
/* Timer section */
// Timer 1, fast PWM no prescaling (non-inverting mode (start high, switch to low))
outp(BV(COM1B1)|BV(WGM11)|BV(WGM10),TCCR1A);
outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
// Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz)
outp(0x00,OCR1AH);
outp(0x7f,OCR1AL);
outp(0x00,TCNT1H);
outp(0x00,TCNT1L);
outp(0x00,OCR1BH);
outp(0x7f,OCR1BL); // to not start motor-rotation before control
/* Timer 2, 4 times pwm-period, for control sampling, prescaler 8, 28.8 kHz */
outp(BV(WGM21)|BV(CS21),TCCR2);
outp(0x3f,OCR2);
/* Reset timer 2 */
outp(0,TCNT2);
// Enable timer2 compare match interrupts
outp(BV(OCIE2),TIMSK);
//Serial communication
outp(0x00, UCSRA); // USART:
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
/* AREF (AREF is 5V) pin external capacitor, MUX0 for current measurement */
outp(BV(REFS0)|BV(MUX0),ADMUX);
// Enable ADC, start first conversion, prescaler 32, not free running mode
outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Initialize TWI
outp(0x02,TWBR); // set SCL-frequency CPU-freq/(16+2*2)
outp(0x02,TWAR); // slave address
outp(BV(TWEA)|BV(TWEN),TWCR); // enable TWI, enable ACK
//Enable interrupts
sei();
// loop
while (1) {}
}
// 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: {
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_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;
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
// 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
*