Select Git revision
vel_control.c
vel_control.c 6.43 KiB
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>
// reference variables
volatile int32_t ref = 0; // 11 frac bits
volatile int16_t refFlag = 0;
volatile int16_t deltaRef = 2;
volatile int16_t refCount = 0;
// velocity control variables
volatile int32_t u = 0; // 11 frac bits
volatile int32_t v = 0; // 11 frac bits
volatile int8_t brake = 0;
volatile int32_t I = 0; // 11 frac bits
volatile int32_t e = 0; // 11 frac bits
volatile int32_t K = 1200; // 6 frac bits
volatile int32_t Ke = 45; // 6 frac bits
volatile int32_t Ksat = 3; // 6 frac bits
// encoder variables
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
volatile int16_t pos = 0;
volatile int16_t oldPos = 0;
volatile int16_t deltaPos = 0;
volatile int8_t newX;
volatile int8_t newY;
volatile int8_t oldX;
volatile int8_t oldY;
volatile int8_t sum = 0;
// velocity estimation parameters
volatile int32_t velEst = 0; // 5 frac bits
volatile int32_t velEstTemp = 0;
volatile int16_t posTemp = 0;
int16_t a = 116; //7 frac bits
int16_t b = 152; // 5 frac bits
/* 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) {};
}
/* Interrupt service routine for handling incoming bytes on the serial port
might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}
/* Send logged data over Serial connection */
static inline void sendData() {
}
/* Routine used to set the red LED */
void setLED(uint8_t on)
{
if (on) PORTB &= ~0x80; //Turn on
else PORTB |= 0x80; //Turn off
}
/* Routine used to initialize the positional encoders */
void initPos()
{
oldX = ENCODERX;
oldY = ENCODERY;
}
/* Routine used to track the cart position */
void setPos()
{
}
/* Timer 2, Encoder */
SIGNAL(SIG_OUTPUT_COMPARE2) {
deltaPos = 0;
// 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); //Find state
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction
//pos = pos+1;
deltaPos = 1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
//pos = pos-1;
deltaPos = -1;
} else {
brake = 1;
// emergency brake
}
oldX = newX;
oldY = newY;
}
// velocity estimation cut-off frequency 500 Hz
pos = pos+deltaPos; // update oldPos, 0 frac bits on pos and oldPos
}
/* Timer 0, control loop */
SIGNAL(SIG_OUTPUT_COMPARE0) {
sei(); // to enable interupts from timer2
PORTC |= 0x10; // to clock calulation time
// linear velocity estimator
// velocity estimate in mm/s
velEst = (((a*velEst+64)>>7)+b*(pos-oldPos)); // 5 fracbits on velEst
oldPos = pos;
// control error
//e = ref-((int16_t)((velEst+16)>>5)); // mm/s
e = ref-((velEst+16)>>5); // mm/s
v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
//saturation and update integral part of ctrl
if (v > 2047) {
I = I + ((((Ke*e) + (Ksat)*(2047-v))+(1<<1))>>2);
} else if (v < -2048) {
I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2);
} else { I = I + ((Ke*e+(1<<3))>>2);
}
u = (v+8)>>4; //8 frac bits to current loop
// friction compensation
if (ref > 0) {
u = u+10;
} else if (ref < 0) {
u = u-10;
}
// Saturation
if (u > 127) {
u = 127;
} else if (u < -128) {
u = -128;
}
// reference calculations
refCount++;
if (refFlag == 0) {
if (refCount%1 == 0)
ref = ref+deltaRef;
if (refCount == 500) {
refFlag = 1;
deltaRef = -deltaRef;
refCount = 0;
}
} else {
if (refCount%1 == 0)
ref = ref+deltaRef;
if (refCount == 1000) {
refCount = 0;
deltaRef = -deltaRef;
}
}
/*
if (refCount == 1000) {
ref = 0;
}
*/
pos = pos*(1-brake);
// TWI-communication
// send start command
outp(BV(TWINT)|BV(TWEN)|BV(TWSTA),TWCR);
while (!(TWCR&BV(TWINT))) {}
// Contact slave
outp(0x02,TWDR); // slave is 0x02 (sla+w)
outp(BV(TWINT)|BV(TWEN),TWCR);
while (!(TWCR&BV(TWINT))) {}
// Send reference byte
outp((int8_t)(u&0x000000ff),TWDR); // send 8 bits reference
outp(BV(TWINT)|BV(TWEN),TWCR);
while (!(TWCR&BV(TWINT))) {}
// 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));
PORTC &= ~0x10;
}
int main()
{
cli();
//Port directions
outp(0x80,DDRB); // Led output
outp(0x10,DDRC); // timer calculation port
/* Timer section */
// Enable timer2 compare match interrupts
outp(BV(OCIE0)|BV(OCIE2),TIMSK);
/* Timer 2, 59 kHz Prescaler 1 */
outp(BV(WGM21)|BV(CS20),TCCR2);
outp(200,OCR2);
/* Reset timer 2 */
outp(0,TCNT2);
/* Timer 0, 1 kHz Prescaler 64 */
outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
outp(230,OCR0);
/* Reset timer 0 */
outp(0,TCNT0);
//Serial communication
outp(0x00, UCSRA); // USART:
outp(0x98, UCSRB); // USART: RxIntEnable|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 */
// 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 Master TWI
outp(0x10,TWBR); // set SCL-frequency CPU-freq/(16+2*16)
outp(BV(TWEN),TWCR); // enable TWI
// initialize position measurements
initPos();
//Enable interrupts
sei();
// loop
while (1) {}
}