//Lab 6 Motor Motion control with an encoder//Description: Program compensates for stiction for //bidirectional motor in addition to controlling //position with an encoder. The Response curve should//have a smooth start and a smooth finish.////Dan Schwarz////10/12/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 0x82 /* static friction */#define c_kinetic_neg 0x86 /* make the value positive */#define c_static_pos 0x82 /* kinetic friction */#define c_static_neg 0x86 /* 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 6 //1,0,1#define Ki 3 //1,1,0#define TABLE_SIZE 11//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 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 *///FUNCTIONSvoid table_init(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 */}void table_generate(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;}int table_update(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++];outint(point_current);outln("\n");}}count = point_current;return point_current;}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;}void ENC_init(void){ /* initialize the register once */DDRB &= ~_BV(PB0); /* set PB0 as an input */DDRB &= ~_BV(PB1); /* set PB1 as an input */}void ENC_update(void){ /* interrupt driven encoder update *//* Static variables for position calculation */static unsigned char state = 0xFF;unsigned char new_state;new_state = (PINB & (_BV(PB0) | _BV(PB1))); /* Read encoder state *//* Update position value */if (state!=new_state) {switch (state) {case 0x00:if (new_state==0x01)Position++;elsePosition--;break;case 0x01:if (new_state==0x03)Position++;elsePosition--;break;case 0x03:if (new_state==0x02)Position++;elsePosition--;break;case 0x02:if (new_state==0x00)Position++;elsePosition--;break;}state = new_state;}}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.ENC_update();if(db_correct == 0){PWM_update(v_output(controller(table_update(),Position)));} else {PWM_update(v_output(deadband(controller(table_update(),Position))));}}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