/* (C) Eitan Sherer & Kenji Hashimoto ECE476 Final Project: Inverted Pendulum Balancer CONTROLLER Code */ #include #define init_motor_period 300 #define init_motor_duty 150 #define init_sensor_index 128 /* * Global Variable Declarations: */ unsigned int motor_PWM_duty; unsigned int motor_PWM_period; unsigned char motor_break_bit; unsigned char motor_timer; unsigned char sensor_index; unsigned char sensor_channel; /* * Function Declarations: */ void initialize(); // initializes ports, timers, interrupts, and global vars void motor_updateDuty(); // sets motor_PWM_duty value to OCR1AH and OCR1AL void motor_updatePeriod(); // sets motor_PWM_period value to ICR1H and ICR1L void motor_break(char); // controls the motor's break signal void motor_direction(char); // controls the motor's direction signal void motor_controller(char); /* * Flash Memory: */ // Speed Level flash char motor_table_speed[8] = { 0, 20, 33, 42, 50, 58, 67, 75 }; // Timer Length flash char motor_table_time[16] = { 0, // Time=0: 0 sec 4, // Time=1: 0.24 sec 30, // Time=2: 0.49 sec 46, // Time=3: 0.75 sec 61, // Time=4: 1 sec 76, // Time=5: 1.25 sec 92, // Time=6: 1.51 sec 107, // Time=7: 1.75 sec 122, // Time=8: 2 sec 137, // Time=9: 2.24 sec 153, // Time=10: 2.5 sec 168, // Time=11: 2.75 sec 183, // Time=12: 3 sec 203, // Time=13: 3.3 sec 223, // Time=14: 3.65 sec 244 // Time=15: 4 sec }; // flash char motor_table_cmd[3] = { 0b10001001, // Dir = Back, Time = 3, Speed = 5 0b00001000, // Dir = Back, Time = 2, Speed = 2 0b00001001 // Dir = Back, Time = 1, Speed = 1 }; /* * Interrupt Service Routines: */ // interrupt [TIM0_OVF] void timer0_ovfl(void) { if (motor_timer > 0) { motor_timer--; } } // External Interrupt triggered by Encoder's Channel A interrupt [EXT_INT0] void ext_int0_isr(void) { sensor_channel ^= 0x01; if (sensor_channel == 1) { sensor_index--; } } // External Interrupt triggered by Encoder's Channel B interrupt [EXT_INT1] void ext_int1_isr(void) { sensor_channel ^= 0x02; if (sensor_channel == 2) { sensor_index++; } } /* * FUNCTION: main() */ void main(void) { initialize(); DDRA = 0xff; PORTA = 0x00; while(1) { if (sensor_index <= 85) { PORTA = 0b00000001; motor_controller(0); } else if (sensor_index > 85 && sensor_index < 171) { PORTA = 0b00001000; motor_controller(1); } else if (sensor_index >= 171) { PORTA = 0b00100000; motor_controller(2); } } } /* * FUNCTION: initialize() */ void initialize() { // Ports //D.2 Ch A input of Encoder --> triggers INT0 //D.3 Ch B input of Encoder --> triggers INT1 //D.5 OC1A output for motor's PWM signal //D.6 output for motor's direction signal //D.7 output for motor's break signal DDRD = 0b11100000; // Timer 0 TCCR0 = 0b00000101; // Timer 1 /* TCCR1A: [7,6] Compare Output Mode for Channel A = 10 (clear on Comp Match, set at TOP) [5,4] Compare Output Mode for Channel B = 00 (disconnected) [3,2] Force Output Compare for Channel A/B = 00 [1,0] Waveform Generation Mode bits 11 & 10 = 10 (Fast PWM w/ TOP=ICR1A) */ TCCR1A = 0b10000010; /* TCCR1B: [7] Input Capture Noise Canceler = 0 [6] Input Capture Edge Select = 0 [5] Reserved Bit = 0 [4,3] Waveform Generation Mode bits 13 & 12 = 11 (Fast PWM w/TOP=ICR1A) [2,1,0] Clock Select = 100 (clk/256) */ TCCR1B = 0b00011100; /* TIMSK: [7] Timer/Counter2, Output Compare Match Interrupt Enable = 0 [6] Timer/Counter2, Overflow Interrupt Enable = 0 [5] Timer/Counter1, Input Capture Interrupt Enable = 0 [4] Timer/Counter1, Output Compare A Match Interrupt Enable = 0 [3] Timer/Counter1, Output Compare B Match Interrupt Enable = 0 [2] Timer/Counter1, Overflow Interrupt Enable = 0 [1] Timer/Counter0, Output Compare Match Interrupt Enable = 0 [0] Timer/Counter0, Overflow Interrupt Enable = 1 */ TIMSK = 0b00000001; // External Interrupt /* MCUCR - MCU Control Register: [3,2] Interrupt Sense Control 1 = 01 (ISR on logical change) [1,0] Interrupt Sense Control 0 = 01 (ISR on logical change) */ MCUCR = 0b00000101; /* GICR - General Interrupt Control Register: [7] External Interrupt Request 1 Enable = 1 [6] External Interrupt Request 0 Enable = 1 [5] External Interrupt Request 2 Enable = 0 */ GICR = 0b11000000; // Global Variables motor_PWM_period = init_motor_period; motor_updatePeriod(); motor_PWM_duty = init_motor_duty; motor_updateDuty(); motor_break_bit = 1; motor_break(motor_break_bit); motor_timer = 0; sensor_index = init_sensor_index; // Enable Interrupts #asm ("sei"); } /* * MOTOR FUNCTION: motor_controller(char param) */ void motor_controller(char param) { char cmd; cmd = motor_table_cmd[param]; // decode command and set parameters // SPEED motor_PWM_duty = (int)(motor_table_speed[(cmd & 0b00000111)]*motor_PWM_period/100); motor_updateDuty(); // TIME motor_timer = motor_table_time[((cmd >> 3) & 0b00001111)]; // DIRECTION motor_direction((cmd >> 7) & 0b00000001); // break OFF if (motor_PWM_duty != 0) motor_break(0); // Wait until timer runs out while (motor_timer > 0) {} // break ON motor_break(1); } /* * MOTOR FUNCTION: motor_break(char param) * param: 0 - break OFF * 1 - break ON */ void motor_break(char param) { switch (param) { case 0: // break OFF PORTD = PORTD & 0b01111111; break; case 1: // break ON PORTD = PORTD | 0b10000000; break; default: break; } } /* * MOTOR FUNCTION: motor_direction(char param) * param: 0 - FORWARD * 1 - BACK * 2 - toggle */ void motor_direction(char param) { motor_break(1); switch (param) { case 0: // FORWARD PORTD = PORTD & 0b10111111; break; case 1: // BACK PORTD = PORTD | 0b01000000; break; case 2: // toggle PORTD = PORTD ^ 0b01000000; break; default: break; } motor_break(motor_break_bit); } /* * MOTOR FUNCTION: motor_updateDuty() */ void motor_updateDuty() { char lowByte; char highByte; lowByte = (char)(motor_PWM_duty); highByte = (char)(motor_PWM_duty >> 8); OCR1AH = highByte; OCR1AL = lowByte; } /* * MOTOR FUNCTION: motor_updatePeriod() */ void motor_updatePeriod() { char lowByte; char highByte; lowByte = (char)(motor_PWM_period); highByte = (char)(motor_PWM_period >> 8); ICR1H = highByte; ICR1L = lowByte; }