//Lab 8 Motor system modeling//Description: Program compensates for stiction for //bidirectional motor in addition to controlling //position with an potentiometer////Dan Schwarz////10/26/2006////INCLUDE STATEMENTS#include <stdio.h>#include <math.h>#include <avr/io.h>#include <avr/signal.h>#include <avr/interrupt.h>#include "sio.c"//PROGRAM CONSTANT DEFINITIONS#define c_kinetic_pos 0x4c // Static Friction#define c_kinetic_neg 0x50 // Make the value positive#define c_static_pos 0x9b // Kinetic Friction#define c_static_neg 0xa1 // Make the value positive#define c_max 255#define c_min 255 /* make the value positive */#define CLK_ms 10 // set the updates for every 10ms, same as CLK_ms#define T 10 /* 4 define as 10 ms, must divide by 1000 later*/#define Kp 10 //1,0,1#define Ki 1 //1,1,0#define TABLE_SIZE 15#define Vmax#define Vmin//GLOBAL DEFINITIONSint moving = 0; //assume start without movingint e_sum = 0;int Position = 0; //Global position variable (feedback)unsigned int CNT_timer1; // the delay timevolatile unsigned int CLK_ticks = 0; // the current number of msvolatile unsigned int CLK_seconds = 0; // the current number of secondsint count = 0; //a count value to be output on portB (AKA. Cd)int db_correct = 0; /* deadband correction is off by default */int ticks; /* variables to keep a system clock count */int desired_position = 0; /* a global variable to track position */int A2DConvert = 0;int j = 0;int table[100];int table_flag = 0;//FUNCTIONSint AD_read(void){return (ADCW >> 2); // Returns digital output from AD register}void create_array(void){table[j] = AD_read();j++;if(j > 99){j = 0;}if(j == 99){table_flag = 0;}}void AD_setup(void){ADMUX = 0xC0; // Set the input channel to zeroADCSRA = 0xE0; // Turn on the ADC and set to free running}int init(void){//initialize the register onceDDRB = 0x04; //set port B as inputsPORTB = 0x04;return 0;}int integrate(int e){e_sum += e*T;if(e_sum > 10000) e_sum = 10000; // set an upper limitif(e_sum < -10000) e_sum = -10000; // set a lower limitreturn e_sum;}int controller(int Cd, int Cf){int Ce;int Cw;Ce = Cd - Cf;Cw = Kp * Ce + Ki * integrate(Ce) / 1000;return Cw;}int deadband(int c_wanted){/* call this routine when updating */int c_pos;int c_neg;int c_adjusted;if(moving == 1){c_pos = c_kinetic_pos;c_neg = c_kinetic_neg;} else{c_pos = c_static_pos;c_neg = c_static_neg;}if(c_wanted == 0){ /* turn off the output */c_adjusted = 0;} else if(c_wanted > 0){ /* a positive output */c_adjusted = c_pos + (unsigned)(c_max - c_pos) * c_wanted / c_max;if(c_adjusted > c_max) c_adjusted = c_max;} else { /* the output must be negative */c_adjusted = -c_neg - (unsigned)(c_min - c_neg) * -c_wanted / c_min;if(c_adjusted < -c_min) c_adjusted = -c_min;}return c_adjusted;}void PWM_init(void){ /* call this routine once when the program starts */DDRD |= (1 << PD5); /* set PWM outputs */DDRC |= (1 << PC0) | (1 << PC1); /* set motor direction outputs on port C*///using OCR1TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);// turn on both PWM outputs on counter 1TCCR1B = _BV(CS11) ; // set the internal clock to /8}void PWM_update(int value){ /* to update the PWM output */if(value > 255) value = 255;if(value < 0) value = 0;OCR1A = value; // duty cycle}int v_output(int v_adjusted){ /* call from the interrupt loop */int RefSignal; // the value to be returnedif(v_adjusted >= 0){ /* set the direction bits to CW on, CCW off */PORTC = (PINC & 0xFC) | 0x02; /* bit 1 on, 0 off */if(v_adjusted > 255){ /* clip output over maximum */RefSignal = 255;} else{RefSignal = v_adjusted;}} else { /* need to reverse output sign *//* set the direction bits to CW off, CCW on */PORTC = (PINC & 0xFC) | 0x01; /* bit 0 on, 1 off */if(v_adjusted < -255){ /* clip output below minimum */RefSignal = 255;} else {RefSignal = -v_adjusted; /* flip sign */}}return RefSignal;}void IO_update(void){ // This routine will run once per interrupt for updates//updatefunction.if(db_correct == 0){PWM_update(v_output(controller(desired_position,AD_read())));} else {PWM_update(v_output(deadband(controller(desired_position,AD_read()))));}if(table_flag == 1){create_array();}}SIGNAL(SIG_OVERFLOW0){ // The interrupt calls this functionCLK_ticks += CLK_ms;if(CLK_ticks >= 1000){ // The number of interrupts between output changesCLK_ticks = CLK_ticks - 1000;CLK_seconds++;}IO_update();TCNT0 = CNT_timer1;// TCNT1 = CNT_timer1;sei();}void CLK_setup(void){ // Start the interrupt service routineTCCR0 = (0<<FOC0) | (0<<WGM01)| (0<<WGM00) | (0<<COM00)| (0<<COM01) | (1<<CS02) | (0<<CS01) | (1<<CS00);// use CLK/1024 prescale value// disable PWM and Compare Output ModesCNT_timer1 = 0xFFFF - CLK_ms * 8; // 8 = 1ms, 80 = 10msTCNT0/*TCNT0*/ = CNT_timer1; // start at the right pointTIFR |= (1<<TOV0);TIFR &= ~(0<<OCF0); //TIFR = (0<<OCF2) | (0<<TOV2) | (0<<ICF1) | (0<<OCF1A) | (0<<OCF1B) | (0<<TOV1) | (0<<OCF0) | (1<<TOV0); // set to use overflow interruptsTIMSK |= (1<<TOIE0);TIMSK &= ~(0<<OCIE0);// TIMSK = (0<<OCIE2) | (0<<TOIE2) | (0<<TICIE1) | (0<<OCIE1A) | (0<<OCIE1B) | // (0<< TOIE1 ) | (0<<OCIE0) | (1<<TOIE0);// enable TCNT1 overflowsei(); // enable interrupts flag}void delay(int ticks){ // ticks are approximately 1msvolatile int i, j;for(i = 0; i < ticks; i++){for(j = 0; j < 1000; j++){}}}void setup(void){PWM_init();sio_init();CLK_setup();init();AD_setup();}int main(void){int c;setup();outln("");outln("--System modeling program");for(;;){while((c = input()) == -1){delay(100);// outint(Position); // outln("this is where the prob is");} // wait for a keypressif(c == 'd'){if(db_correct == 0){db_correct = 1;outln("deadband correction ON");} else {db_correct = 0; outln("deadband correction OFF");}} else if(c == '+'){count++;if(count > 255){count = 255;}outint(count); outln(" = count");} else if(c == '-'){count--;if(count < -255){count = -255;}outint(count); outln(" = count");} else if(c == 'g'){desired_position = count;table_flag = 1;outint(desired_position); outln(" = Desired Position");} else if (c == 'a'){A2DConvert = AD_read();outint(A2DConvert); outln(" = Analogue to digital converter");}else if(c == 't'){int k;for(k = 0; k < 100; k++){outint(table[k]);outln("");}}else if(c == 'h'){outln("+ : increment count");outln("- : decrement count");outln("g : initiate motion using current count value as desired position");outln("a : current analogue to digital converter value");outln("t : output table of ADC readings");outln("");outln("");}}sio_cleanup();return
View Full Document