// Prelab 6 // EGR 345 // 10-7-03 // Martin Przekadzinski // Chris Hough // Irma Montigny // Motion Control #include /* 68HC11E9 I/O register defines */ #include /* Used for serial communication functions */ #include /* PWM library functions */ #include #define T 4 /* define as 4 ms, must divide by 1000 later*/ #define Kp 20 #define Ki 0 #define TABLE_SIZE 11 int new_pos; int integrate(int); void EncoderUpdate(void); int controller(int, int); int deadband(int c_wanted); void v_output(int v_adjusted); int init_table(); void update_table(); void get_setpoint(); void init(); int count =0; int c_wanted = 0; int c_stick_pos = 168; //Compensation for the deadband int c_stick_neg = 164; //Compensation for the deadband int c_max = 255; int c_min = 255; int e_sum; int Position; int point_master[TABLE_SIZE] = {0, 24, 95, 206, 345, 500, 655, 794, 905, 976, 1000}; int point_position[TABLE_SIZE]; int point_time[TABLE_SIZE]; int point_start_time; int point_index; int ticks; /* variables to keep a system clock count */ int point_current; /* a global variable to track position */ int duration; extern void __attribute__((interrupt)) RTI_ISR(void){ TFLG2 = RTIF; /*clear the interrupt flag*/ __asm("cli"); EncoderUpdate(); get_setpoint(); v_output(deadband(controller(point_current, Position))); count++; if(count>127) { outint16(Position); putstr("\n"); count =0; } } int main(void) { *(unsigned *)0x00EC = (unsigned)RTI_ISR; init(); new_pos = 0; unsigned mode; char key; putstr(" Press 'q' to quit\n"); putstr(" Press '0-9' to set duration\n"); putstr(" Press 'a-f' to move the motor\n"); putstr(" Press 'p' to print out position\n"); duration = 0; mode = 0;/*Use polling loop update*/ do { key = input(); switch (key){ case 0:/*nothing received*/ break; case '0': duration = 1; break; case '1': duration = 2; break; case '2': duration = 3; break; case '3': duration = 4; break; case '4': duration = 5; break; case '5': duration = 6; break; case '6': duration = 7; break; case '7': duration = 8; break; case '8': duration = 9; break; case '9': duration = 10; break; case 'a': case 'A': new_pos = -48; break; case 'b': case 'B': new_pos = -32; break; case 'c': case 'C': new_pos = -16; break; case 'd': case 'D': new_pos = 16; break; case 'e': case 'E': new_pos = 32; break; case 'f': case 'F': new_pos = 48; break; case 'g': case 'G': putstr("time ="); outint16(duration); putstr("\n"); putstr("distance ="); outint16(new_pos); putstr("\n"); putstr("\n"); update_table(point_current, point_current +new_pos, duration); break; case 'p': case 'P': putstr("POSITION: "); outint16(point_current); putstr("\n"); break; case 'q': case 'Q': mode = 1;/*quit program*/ break; } }while (mode == 0);/*Continue until quit*/ PWMDisable();/*Disable PWM generator*/ return 0; } int deadband(int c_wanted) { int c_adjusted; if(c_wanted == 0){ //turn off the output c_adjusted = 0; } else if(c_wanted > 0){ //a positive output c_adjusted = c_stick_pos + (unsigned)c_wanted * (c_max - c_stick_pos)/c_max; } else { //the output must be negative c_adjusted = -c_stick_neg - (unsigned)(-c_wanted) * (c_min - c_stick_neg)/c_min; } return c_adjusted; } void v_output(v_adjusted) { if(v_adjusted >= 0){ //set the direction bits to CW on, CCW off PORTD |= PD2; //CW on PORTD &= ~PD3; //CCW off if(v_adjusted >255){ RefSignal = 255; } else { RefSignal = v_adjusted; } } else { //need to reverse the output sign //set the direction bits to CW off, CCW on PORTD |= PD3; //CCW on PORTD &= ~PD2; //CW off if(v_adjusted < -255){ RefSignal = 255; } else { RefSignal = -v_adjusted; //flip sign } } } void init(){ /* initialize the register once */ /*Code for initializing PWM*/ PWMInit(); /*Initializing PWM*/ PWMEnable(); /*Enable PWM generator*/ DDRD |= (DDD2 | DDD3); /* set up the timed interrupt */ *(unsigned *)0x00EC = (unsigned)RTI_ISR; /* interrupt subroutine */ TMSK2 |= RTII; /*enable the interrupt */ __asm("CLI"); /*clear interrupts*/ /* setup the analog input */ OPTION |= ADPU; /*turn on the analog input*/ ADCTL = SCAN | CA; /*scan the inputs continuously */ DDRD &= ~(PD4 | PD5); /* set pins 4 and 5 as inputs */ init_table(); } void EncoderUpdate(void){ /* interrupt driven encoder update */ /* Static variables for position calculation */ static unsigned char state = 0xFF; unsigned char new_state; new_state = (PORTD & (PD5 | PD4)) >> 4; /* Read encoder state */ /* Update position value */ if (state!=new_state) { switch (state) { case 0x00: if (new_state==0x01) Position++; else Position--; break; case 0x01: if (new_state==0x03) Position++; else Position--; break; case 0x03: if (new_state==0x02) Position++; else Position--; break; case 0x02: if (new_state==0x00) Position++; else Position--; break; } state = new_state; } } int integrate(int e){ e_sum += e * T; return e_sum; } int controller(int Cd, int Cf){ int Ce; int Cw; Ce = Cd - Cf; Cw = Kp * Ce + (long int)Ki * integrate(Ce) / 1000; return Cw; } int init_table(void){ /* initialize the setpoint table */ ticks = 0; /* set the clock to zero */ point_current = 0; /* start the system at zero */ point_index = TABLE_SIZE; /* mark the table as empty */ return 0; } void update_table(int start, int end, int duration_sec){ unsigned i; point_time[0] = ticks + 10; /* delay the start slightly */ point_position[0] = start; for(i = 1; i < TABLE_SIZE; i++){ point_time[i] = point_time[0] + (unsigned long)i * duration_sec * 250 / (TABLE_SIZE - 1); point_position[i] = start + (long int)(end - start) * point_master[i] / 1000; } point_index = 0; } void get_setpoint(void){/* interrupt driven encoder update */ ticks++; /* update the clock */ if(point_index < TABLE_SIZE){ if(point_time[point_index] == ticks){ point_current = point_position[point_index++]; //outint16(point_current); } } }