Added folders for Embedded Control course

linear_pendulum_2009/avr/Makefile

+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

linear_pendulum_2009/avr/current_control.c

+/*********************************************
+
+ 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
+			}
+		} 
+	}
+}