Skip to content
Snippets Groups Projects
Commit 829752b6 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Almost final commit

parent d941627b
Branches
No related tags found
No related merge requests found
/*********************************************
Current regulation - Aleks Ponjavic 18/09/07
for LTH - reglerteknik
Note that the voltage is actually a duty
cycle for the PWM. The PWM is running at
12V with a duty cycle set by a 10bit value
*********************************************/
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>
// reference generation variables
volatile int16_t ref = 100;
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
// control variables
volatile unsigned char lbyte,hbyte;
volatile int16_t y;
volatile uint8_t alt = 1;
volatile uint8_t low, high;
//test variables
volatile int16_t e = 0;
volatile int16_t v = 0;
volatile int16_t I = 0;
volatile int16_t u = 0;
volatile int16_t K = 74;//5;//375; // 7 frac bits
volatile int16_t Ke = 26;//2;//6; //7 frac bits, K*h/Ti
volatile int16_t Ksat = 44;//1; // 7 frac bits, h/Tr
// 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;
volatile int16_t temp;
// pwm sync variables
volatile int8_t pwmCycles = 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) {};
}
/* Interrupt service routine for handling incoming bytes on the serial port
might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}
static inline int16_t readInput() {
uint8_t low, high;
ADCSRA |= 0x40;
while (ADCSRA & 0x40);
low = ADCL;
high = ADCH;
return ((high<<8) | low) - 512;
}
/* Write 8-bit output using the PWM-generator */
static inline void writeOutput(int16_t val) {
if (val < 0) {
PORTC = 0x80+(PORTC & 0x7F);
OCR1BH = 0; //(unsigned char) (-val)&0xff00;
OCR1BL = (unsigned char) (-val)&0x00ff;
} else {
PORTC = (PORTC & 0x7F);
OCR1BH = 0; //(unsigned char) (val&0xff00);
OCR1BL = (unsigned char) (val&0x00ff);
}
}
/* 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 1 compare match, start AD-conversion
* Additional changes: Dont use free running AD-conversion
* Enable overflow interrupts timer1
*/
SIGNAL(SIG_OUTPUT_COMPARE2) {
// Start AD conversion
ADCSRA |= BV(ADSC);
// Code previously in sig_adc
PORTA |= 0x10; //Turn on calculation timer
// Read input
low = inp(ADCL);
high = inp(ADCH);
y = ((high<<8) | low) - 512; //y 9 frac bits
//control, since negative measurements
e = ref+y; //e 9 frac bits
//v = (int16_t)((K*e+I+64)>>7);
v = (int16_t)(((K*e+64)>>7)+(temp>>4));
//saturation and update integral part of ctrl
if (v > 511) {
temp = temp + (((Ke*e) + (Ksat)*(511-v))>>3);
} else if (v < -512) {
temp = temp + (((Ke*e) + (Ksat)*(-512-v))>>3);
} else {
temp = temp + ((Ke*e)>>3);
}
/*
//saturation and update integral part of ctrl
if (v > 511) {
I = I +(int32_t)((Ke*e) + (Ksat)*(511-v));
} else if (v < -512) {
I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
} else {
I = I +(int32_t)(Ke*e);
}
*/
// write output, inverting mode means set pwm to 127-ctrl_out
// Original styrning med 7 bitar + direction
u = (v+2)>>2; //7 frac bits to pwm
if (u > 127) {
u = 127;
} else if (u < -128) {
u = -128;
}
if (u < 0) {
PORTC |= 0x80;
OCR1BL = (unsigned char) (128-(-u));
} else {
PORTC = (PORTC & 0x7F);
OCR1BL = (unsigned char) (127-u);
}
// For logging
countSamples++;
if (countSamples == skipSamples) {
ctrl_log[jj] = y;
I_log[jj] = u;
error_log[jj] = e;
jj++;
countSamples = 0;
// Stop after a while
if ((jj == (log_len-1)) & !stop) {
outp(0x7f,OCR1BL);
stop = 1;
sendData();
}
}
PORTA &= 0xef; //Turn off calculation timer
}
/* reference square- or triangle wave generator with timer 0 */
SIGNAL(SIG_OVERFLOW0) {
int8_t rectangle = 1;
refCount++;
if (rectangle == 1) {
if (refFlag == 0) {
if (refCount == 20) {
refFlag = 1;
ref = -ref;
refCount = 0;
}
} else {
if (refCount == 40) {
ref = -ref;
refCount = 0;
}
}
} else {
if (refCount <= 200) { // ref
ref -= 1;
} else {
ref += 1;
}
if (refCount == 400) { // ref*2
refCount = 0;
}
}
}
int main()
{
cli();
int i,j;
//Port directions
outp(0x08,PORTC); // pull up on overtemp signals
outp(0xa0,DDRC); // output on dir and brake
outp(0x10,DDRD); // output on pwm for motor 1
outp(0x10,DDRA); // test pin output
/* Timer section */
// Enable timer0 overflow interrupts
// and enable timer1 overflow interrupts for pwm-control sync
outp(BV(TOIE0)|BV(OCIE2),TIMSK);
// Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high))
outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|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-> h_pwm=8.61 micros)
// 7 bit ctrl-signal and direction
outp(0x00,OCR1AH);
outp(0x7f,OCR1AL);
outp(0x00,TCNT1H);
outp(0x00,TCNT1L);
// 7 bit ctrl-signal and direction
outp(0x00,OCR1BH);
outp(0x7f,OCR1BL); // to not start motor-rotation before control
/* Timer 2, 4 times pwm-period, for control sampling, prescaler 8 */
outp(BV(WGM21)|BV(CS21),TCCR2);
outp(0x3f,OCR2);
/* Reset timer 2 */
outp(0,TCNT2);
/* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */
outp(BV(CS02)|BV(CS00),TCCR0);
//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, MUX3?? for pendulum angle */
outp(BV(REFS0)|BV(MUX0),ADMUX);
// Enable ADC interrupts, start first conversion, prescaler 32, free running mode
//outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC interrupts, start first conversion, prescaler 32, not free running mode
//outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC, start first conversion, prescaler 32, not free running mode
outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// outp(BV(ADEN)|BV(ADSC),ADCSRA);
/* Wait a little bit, probably not needed...*/
int tmp;
// for(i=0;i<2000;i++)
// for(j=0;j<400;j++)
// tmp = j*j*j;
//Enable interrupts
sei();
// loop
while (1) {
if (stop) {
cli();
OCR1BL = 0xff;
}
}
}
/***************************************************************
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>
......@@ -11,22 +17,19 @@ 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;
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;
volatile int8_t intCond = 0; // flag for conditional integration
#define V_MAX 508
#define V_MAX 508 //max/min for saturation
#define V_MIN -508
// twi variable
volatile int16_t status;
// logging variables
/*
#define log_len 100
......@@ -39,9 +42,6 @@ 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)
......@@ -80,7 +80,6 @@ static inline void sendData() {
/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL(SIG_OUTPUT_COMPARE2) {
PORTB |= 0x40;
// Start AD conversion
ADCSRA |= BV(ADSC);
......@@ -159,7 +158,6 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
}
*/
PORTB &= ~0x40;
}
......@@ -175,7 +173,7 @@ int main()
outp(0x40,DDRB); // temp pwm output
/* Timer section */
// Timer 1, fast PWM no prescaling (non-inverting mode (start low, switch to high))
// 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);
......
......@@ -24,8 +24,8 @@ volatile int32_t e = 0; // 11 frac bits
volatile int8_t intCond = 0;
volatile int32_t K = 800; // 6 frac bits, prop constant
volatile int32_t Ke = 30; // 6 frac bits, integral constant
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)
......@@ -56,7 +56,6 @@ 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() {
cli();
......@@ -137,6 +136,8 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
sei(); // enable interrupts from encoder-counter
// Poll UART receiver
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
......@@ -154,6 +155,8 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
if (toSend >= 0) UDR = (char)toSend;
}
TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl)
TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts
......@@ -221,11 +224,16 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
uSend = u;
sei();
// TWI-communication to set current reference on the other atmel
// 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);
......@@ -236,7 +244,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
outp(BV(TWINT)|BV(TWEN),TWCR);
while (!(TWCR&BV(TWINT))) {}
// stop transmission
outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);
......@@ -272,9 +279,10 @@ int main()
outp(0,TCNT1L);
/* Timer 0, 10 kHz, Prescaler 8, serial communication */
/* Timer 0, 40 kHz, Prescaler 8, serial communication */
outp(BV(WGM01)|BV(CS01),TCCR0);
outp(184-1,OCR0); // 10 kHz
//outp(184-1,OCR0); // 10 kHz
outp(46-1,OCR0); // 40 kHz
/* Reset timer 0 */
outp(0,TCNT0);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment