Select Git revision
vel_control.c
-
Anders Blomdell authoredAnders Blomdell authored
vel_control.c 11.98 KiB
/*
**************************************************************
Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
for LTH - reglerteknik
***************************************************************
*/
#include <util/twi.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.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
struct {
volatile uint16_t samples; // nbr of samples since last ctrl-ref update
struct {
volatile unsigned char pending;
volatile int32_t next;
volatile int32_t value;
} vel;
struct {
volatile int32_t value;
} acc;
} ref = {
.samples = 0,
.vel.pending = 0,
.vel.next = 0,
.vel.value = 0,
.acc.value = 0,
};
volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
// velocity control variables
volatile int32_t u = 0; // 11 frac bits
volatile int8_t brake = 0; // brake variable if pos-sample missed
volatile int32_t I = 0; // 11 frac bits
volatile int32_t K = 807; // 6 frac bits, prop constant
volatile int32_t Ke = 13; // 6 frac bits, integral constant
volatile int8_t fr_comp = (10<<3);
#define V_MAX (120<<4)
#define V_MIN (-120<<4)
// encoder variables
#define ENCODERY (PIND&0x04) //Positional encoder pins
#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins (shift one step for faster comparison with Y)
// position variables
volatile int16_t pos = 0; // position in tics
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
volatile int8_t newY; // encoder signal
volatile int8_t oldX; // previous encoder signal
volatile int8_t oldY; // previous encoder signal
// velocity estimation parameters
volatile int32_t velEst = 0; // vel-estimate, 5 frac bits
int16_t a = 116; // 7 frac bits (parameter in velocity estimate)
int16_t b = 152; // 5 frac bits (parameter in velocity estimate)
// adc measurement variables
volatile int16_t low; // temporary variable for ad-reading
volatile int16_t high; // temporary variable for ad-reading
volatile int16_t angleOffset = 0;
/* return position (in tics) */
int32_t getPosition() {
int16_t result = 0;
cli();
result = pos;
sei();
return (result);
}
/* return velocity (in mm/s) */
int32_t getVelocity() {
int32_t result;
cli();
result = velEst;
sei();
return result;
}
/* return last angle measurement */
int16_t getAngle() {
int16_t adc = ADC;
while (adc != ADC) {
adc = ADC;
}
return adc - 512 - angleOffset;
}
/* return current-reference */
int32_t getCurrentRef() {
int32_t result;
cli();
result = u;
sei();
return result;
}
/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
// Called from serial interrupt, so should be atomic by itself
if (ref.vel.pending) {
ref.vel.pending = 0;
ref.vel.value = ref.vel.next;
ref.acc.value = newAccRef;
ref.samples = 0;
} else {
// TODO: report error
}
}
/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
// Called from serial interrupt, so should be atomic by itself
ref.vel.pending = 1;
ref.vel.next = newVelRef;
}
/* Routine used to initialize the positional encoders */
void initPos()
{
oldX = ENCODERX;
oldY = ENCODERY;
}
/* 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
/*
sum = (oldX<<2)+oldY+newX+(newY>>2);
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
pos = pos+1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
pos = pos-1;
} else {
brake = 1; // emergency brake
}
*/
// Works like if-clauses above, but faster!
if ((oldX == newY) && (oldY != newX)) {
pos = pos+1;
} else if ((oldX != newY) && (oldY == newX)) {
pos = pos-1;
} else {
brake = 1;
}
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;
}
}
SIGNAL(TWI_vect) {
unsigned char twsr = TWSR;
switch (twsr) {
case 0x08: {
TWDR = 0x02; // slave is 0x02 (sla+w)
TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
} break;
case 0x18:
case 0x20: {
TWDR = u;
TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
} break;
case 0x28:
case 0x30: {
TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWSTO)|_BV(TWIE);
} break;
default: {
// This should never happen
TWCR = (_BV(TWINT)|_BV(TWEN)|_BV(TWIE));
} break;
}
}
/* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) {
if (ref.samples <= 500) {
ref.samples++;
}
posCtrl = pos; // store pos to use in this loop
int32_t vel_ref = ref.vel.value;
int32_t acc_ref = ref.acc.value;
int16_t samples = ref.samples;
sei(); // to enable interupts from encoder counter, serial and TWI
// velocity estimation in mm/s
velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
oldPos = posCtrl;
if (samples > 500) {
// Communication lost, stop and reset
I = 0; // Protect u and TWCR for concurrent updates
cli();
u = 0;
} else {
// store velEst and ref to be sent/used here
refCtrl = vel_ref + ((acc_ref*samples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
// control error
int32_t e = refCtrl-((velEst+16)>>5); // mm/s
// temporary ctrl-signal
int32_t v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
// friction compensation
if (refCtrl > 0) {
v = v+fr_comp;
} else if (refCtrl < 0) {
v = v-fr_comp;
}
// variable that decides if I-part should be updated
int8_t intCond = 1;
// saturation of v
int32_t vSat;
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<<1))>>2);
}
// Protect u and TWCR for concurrent updates
cli();
// scale ctrl-signal to send over twi
u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
}
// TWI-communication to set current reference on the other atmel
// send start command
if (TWCR == (_BV(TWEN)|_BV(TWIE)) || TWCR == 0) {
TWCR = _BV(TWEN)|_BV(TWSTA)|_BV(TWIE);
}
}
int main()
{
cli();
//Port directions
PORTD = 0x40; // pull up on reset switch
/* Timer section */
// Enable timer1, timer2 compare match interrupts
TIMSK = _BV(OCIE1A)|_BV(OCIE2);
/* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
TCCR2 = _BV(WGM21)|_BV(CS20);
OCR2 = 200;
/* Reset timer 2 */
TCNT2 = 0;
/* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
TCCR1B = _BV(WGM12)|_BV(CS10);
OCR1AH = 0x38;
OCR1AL = 0x3f;
TCNT1H = 0;
TCNT1L = 0;
/* Timer 0, 40 kHz, Prescaler 8, serial communication */
TCCR0 = _BV(WGM01)|_BV(CS01);
//OCR0 = 184-1; // 10 kHz
OCR0 = 45-1; // 40 kHz
/* Reset timer 0 */
TCNT0 = 0;
// syncronization (ctrl interrupt 34 micros before communication interrupt)
TCNT1 = TCNT0*8+500;
//Serial communication initialization
UCSRA = 0x00; // USART:
UCSRB = 0x98; // USART: RxIntEnable|RxEnable|TxEnable
UCSRC = 0x86; // USART: 8bit, no parity
UBRRH = 0x0; // USART: 115200 @ 14.7456MHz
UBRRL = 7; // USART: 115200 @ 14.7456MHz
/* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
ADMUX = _BV(REFS0)|_BV(MUX0)|_BV(MUX1);
// Enable ADC on adc3, start first conversion, prescaler 128, free running mode
ADCSRA = _BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
// Initialize Master TWI
TWBR = 0x10; // set SCL-frequency CPU-freq/(16+2*16)
TWCR = _BV(TWEN)|_BV(TWIE);
// initialize position measurements
initPos();
// initialize pc-communication
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();
low = ADCL;
high = ADCH;
pos = 0; // reset position
angleOffset = ((int16_t) ((high<<8) | low) - 512);
oldPos = 0;
velEst = 0; // reset velocity estimate
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();
}
}
}