Commit 316030e8 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Removed some old files

parent 9021297a
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
/*********************************************
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 <math.h> //Only include if sin will be used, takes up memory!
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
//Main control parameters
#define MODE 3 //1 = velocity, 2 = current, 3 = cascade, 0 = control off
#define U_MAX 400 //Voltage saturation point
#define U_FIXEDPOINT 3000 //Wanted current, used when MODE = 2, note this is in fixed point arithmetic 2^6
#define U_REAL 100 //When MODE = 1, voltage used for left-right motion
#define NBR_CHAN 1
//Current regulation parameters
#define KC_P 2 //Proportional constant
#define KC_I 0 //Integral constant (currently 0 as PI_C is run as many times as PI_V integral control
// -does not make sense when in cascade) for MODE = 2, KC_I = 5 is ok
#define ANTI_WINDUP 10 //Anti windup constant
//Current regulation variables
volatile long v_C = 0; //Voltage
volatile long e_C = 0; //Error
volatile long I_C = 0; //Integral part
volatile long a_C = 0; //Current (amperes)
volatile long u_C = 0; //Used for anti windup
volatile long U = 0; //= +/- U_FIXEDPOINT, used for current regulation, MODE = 2
//Velocity regulation parameters
#define V_REQ 70 //Required velocity, note the magnitude of this depends on STEP
#define KV_P 8 //Proportional constant
#define KV_I 2 //Integral constant
//Velocity regulation variables
volatile long v_V; //Voltage
volatile long e_V = 5; //Error
volatile long I_V = 0; //Integral part
volatile long u_V = 0;
volatile long VEL = 0; //= +/- V_REQ
//Position/velocity parameters
#define STEP 100 //Amount of steps between position measurements, used for determining velocity
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
//Position/velocity variables
volatile int positions[STEP]; //Array holding history of position at each sample
volatile int currentPos; //Current sample
volatile long vel = 0; //Current velocity
volatile int oldX; //Used for positional encoders
volatile int oldY; //Used for positional encoders
volatile int sum; //Used for positional encoders
volatile long pos = 0; //Current position
//Sampling variables
volatile int sendData[8]={0,7,6,5,4,3,2,1}; //Array holding data that is sent through serial port
volatile char dataAvailable = 0; //Flag if an unhandled ADC has been completed
volatile int nbrSampSinceLastSend = 0; //Checks if a sample has been missed and if so discards it
volatile char logStarted = 0; //Flag
volatile int dir = 0; //Direction of motion (+ = right, - = left)
/* 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()
{
int newX = ENCODERX;
int 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++;
else
pos--;
oldX = newX;
oldY = newY;
}
}
/* Routine used to set the PWM duty cycle */
void setMotorVoltage(int axis, int u)
{
int pwm;
u >>= 1; //Taking specified 10bit value but PWM is 9bit, therefore compensate
if (axis==0) //Axis determines which motor is used but currently only one motor available
{
if (u < 0) { //Determine direction
pwm = -u;
PORTC |= 0x80;
}
else
{
pwm = u;
PORTC &= ~0x80;
}
if (pwm > 0x3ff) { pwm = 0x3ff; }
OCR1B = pwm & 0x3ff; //Set PWM duty cycle
}
}
/* 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) {};
}
/* Routine for updating the motor voltage input with current control */
void PI_C(long input, long ref)
{
//Convert adc input to current. Calculations listed in trac-wiki
a_C = input*5-2601; //Get current
a_C*=3; //Convert it to
e_C = ref+a_C;
v_C = KC_P*(e_C)+KC_I*I_C+60*dir; //Set voltage
v_C = (v_C >> 6); //Remove fixed point arithmetic
if(v_C <=U_MAX && v_C>-U_MAX) //Check for saturation
{
u_C = v_C;
setLED(1); //Notify that PI is running
}
else
{
if(v_C<0) u_C=-U_MAX;
else u_C=U_MAX;
setLED(0);
}
if((e_C>>5)==0) //Depending on sign 0 gets rounded as 0 or -1, compensate
I_C+= 1 + ANTI_WINDUP*(u_C-v_C);
else
I_C += (e_C >> 5) + ANTI_WINDUP*(u_C-v_C); //Update integral part
setMotorVoltage(0,v_C); //Set voltage
}
/* Routine for updating the input for the current control with speed regulation */
void PI_V(long input)
{
e_V = (VEL<<4)-(input<<4); //Calculate error
v_V = KV_P*e_V + KV_I*I_V; //Set voltage
v_V = (v_V >> 4)+60*dir; //Remove fixed point arithmetic and add friction compensation
if(v_V <=U_MAX && v_V>-U_MAX) //Check for saturation
{
u_V = v_V;
setLED(1); //Notify that PI is running
}
else
{
if(v_V<0) u_V=-U_MAX;
else u_V=U_MAX;
setLED(0); //Notify saturation
}
if((e_V>>6)==0) //Again, sign rounding compensation
I_V++;
else
I_V += e_V>>6; //Update integral part
if(MODE==1)setMotorVoltage(0,u_V); //Set voltage if in velocity control, otherwise cascaded current will set voltage
}
/* Interrupt service routing for handling timer 1 */
SIGNAL(SIG_OVERFLOW1)
{
static long ctr1;
static long ctr2;
ctr1++;
//Sinewave motion in steps
if(ctr1>=3)
{
outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSR); //Request ADC
ctr2++;
if(ctr2>=8000) //Delay between direction switch
{
//Right-left motion
if(dir==-1)
{
if(MODE) //Check that control is on
{
U = -U_FIXEDPOINT; //Set required current (in fixed point arithmetic)
VEL = -V_REQ; //Set required velocity
I_C = 0; //Reset integral part
I_V = 0; //Reset integral part
}
else
{
setMotorVoltage(0,-U_REAL); //If no control simply set voltage to required value
}
dir = 1; //Change direction
}
else
{
if(MODE) //Repeat of previous if statement but in reverse direction
{
U = U_FIXEDPOINT;
VEL = V_REQ;
I_C = 0;
I_V = 0;
}
else
{
setMotorVoltage(0,U_REAL);
}
dir = -1;
}
ctr2=0; //Reset counter
}
ctr1=0; //Reset counter
}
}
/* Not currently in use but necessary to catch interrupt on timer0 overflow */
SIGNAL(SIG_OVERFLOW0)
{
static int ctr0;
ctr0++;
if (ctr0>=10)
{
//putchar('O');
ctr0 = 0;
}
}
/* Interrupt service routine for handling ADC interrupts */
SIGNAL(SIG_ADC)
{
static int ctrA;
unsigned char lbyte,hbyte;
ctrA++;
lbyte = inp(ADCL); //Get ADC result low byte
hbyte = inp(ADCH); //Get ADC result high byte
if (ctrA>=1)
{
if (logStarted == 1)
{
cli();
dataAvailable = 1; //Flag data being available
nbrSampSinceLastSend++; //Increase samples since last send
sendData[0] = (int)((hbyte<<8)+lbyte); //Store result in data array
sei();
}
else
{
dataAvailable = 1;
sendData[0] = (int)((hbyte<<8)+lbyte);
}
ctrA = 0;
}
}
/* Interrupt service routine for handling incoming bytes on the serial port */
SIGNAL(SIG_UART_RECV)
{
char ch = inp(UDR); //Read input
cli();
logStarted = 1; //Flag log started
sei();
}
/* Interrupt service routine for timer 0; Not in use */
/*SIGNAL(SIG_OVERFLOW0)
{
static int ctr0;
ctr0++;
if (ctr0>=10)
{
//putchar('O');
ctr0 = 0;
}
}*/
int main()
{
int i,j; //Used for for loops
//Port directions
outp(0x80,PORTB); // LED off
outp(0x80,DDRB); // output on LED
outp(0x0c,PORTC); // pull up on overtemp signals
outp(0xf0,DDRC); // output on dir and brake
outp(0x80,PORTD); // pull up on reset switch
outp(0x30,DDRD); // output on pwm1&2
//Counters
outp(BV(TOIE0)|BV(TOIE1),TIMSK);// Enable TCNT0 and TCNT1 overflow interrupts
outp(0x00,TCNT0); // Reset TCNT0, CLK/1024
outp(0x00,TCNT1H); // Reset TCNT1 high, CLK/512
outp(0x00,TCNT1L); // Reset TCNT1 low, CLK/512
outp(BV(CS02)|BV(CS00), TCCR0); // 1024
//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
// Timer 1 (PWM) 14.7456MHz/1/512 -> 28.8kHz
// PWM needs to run at 1/512 in order to avoid vibration excitation
// OC1A & OC1B 9 bit fast PWM, active high
outp(BV(COM1A1)|BV(COM1B1)|BV(WGM11),TCCR1A);
outp(BV(WGM12)|BV(CS10),TCCR1B); // No prescaler, or /1...
outp(BV(REFS0)|BV(MUX0),ADMUX); //AREF (AREF is 5V) pin external capacitor, MUX0 for current, MUX3 for pendulum angle
// Enable ADC interrupts, CLK/128
outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSR);
unsigned int data[8]={1,2,3,4,5,6,7,8}; //Reset data array
/* 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;
//Initialize encoder
initPos();
//Intialize velocity
for(j=0;j<STEP;j++)
positions[j]=0;
currentPos = STEP;
sei(); //Enable interrupts
for(;;)
{
//Update positional encoder
setPos();
if(dataAvailable && !logStarted)
{
dataAvailable = 0; //Reset flag
//Update velocity
vel = pos - positions[(currentPos+1)<STEP ? currentPos+1 : 0]; //Determine velocity
positions[currentPos]=pos; //Update position array
currentPos++; //Update current sample
if(currentPos>=STEP)
currentPos-=STEP; //If overflow, loop around
if(MODE==1||MODE==3)PI_V(vel); //Velocity or cascade control
if(MODE==3)PI_C(sendData[0], u_V<<6); //Cascade control, voltage is put in fixed point arithmetic
if(MODE==2)PI_C(sendData[0], U); //Velocity control
}
while(logStarted==1 && dataAvailable==1)
{
cli(); // Enter critical section: disable interrupts
for(i=0;i<NBR_CHAN;i++)
{
data[i] = (unsigned int)sendData[i]; //Store data locally
}
//Update velocity
vel = pos - positions[(currentPos+1)<STEP ? currentPos+1 : 0]; //Determine velocity
positions[currentPos]=pos; //Update position array
currentPos++; //Update current sample
if(currentPos>=STEP)
currentPos-=STEP; //If overflow, loop around
data[2] = (unsigned int)vel; //Parameter that can be sent through SerialLogger
if (nbrSampSinceLastSend>1)
data[0] = 0; //Check if sample was missed
dataAvailable = 0; //Reset flag
nbrSampSinceLastSend = 0;
sei(); // Exit critical section: enable interrupts
if(MODE==1||MODE==3)PI_V(vel); //Velocity or cascade control
if(MODE==3)PI_C(sendData[0], u_V<<6); //Cascade control, voltage is put in fixed point arithmetic
if(MODE==2)PI_C(sendData[0], U); //Velocity control
// Transmit data
for(i=0;i<NBR_CHAN;i++)
{
putchar((unsigned char)((data[i+2]&0xff00)>>8)); //Low byte
putchar((unsigned char)((data[i+2]&0x00ff))); //High byte
}
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment