//============================================================================ // COBRA Microcontroller Firmware // 3/14/2009 | Version 1.5.5 -> 04/22/2009 // Written by: Chris Miller and Nick Viera //============================================================================ // CHANGES in this firmware revision: /* -- General code clean-up -- reduce steering sensitivity -- Fixed bug to allow braking (regen) to actually work. -- Allow for main relay toggling -- Enable full braking power and increase breaking "region" on controls -- Revert back to linear controls -- Add "death spin" cabability */ #include #include #include // =========================================================================== // Macro Definitions - for easier code and I/O manipulation // =========================================================================== // Bitwise operation macros #define BIT(x) (0x01 << (x)) #define bit_get(p,m) ((p) & (m)) #define bit_set(p,m) ((p) |= _BV(m)) #define bit_clr(p,m) ((p) &=~ _BV(m)) #define bit_tog(p,m) ((p) ^= _BV(m)) // LED macros #define led_power_on bit_set(PORTC, 1) #define led_power_off bit_clr(PORTC, 1) #define led_power_tog bit_tog(PORTC, 1) #define led_bind_on bit_set(PORTC, 2) #define led_bind_off bit_clr(PORTC, 2) #define led_bind_tog bit_tog(PORTC, 2) // Motor Control Macros #define motor_left_pwm OCR1A #define motor_left_rev bit_clr(PORTC, 3) #define motor_left_for bit_set(PORTC, 3) #define motor_left_braking OCR0A #define motor_right_pwm OCR1B #define motor_right_rev bit_clr(PORTC, 4) #define motor_right_for bit_set(PORTC, 4) #define motor_right_braking OCR0B #define motor_weapon_on bit_set(PORTB, 5) #define motor_weapon_off bit_clr(PORTB, 5) // Other Relay Macros #define relay_main_on bit_set(PORTB, 3) #define relay_main_off bit_clr(PORTB, 3) #define bind_on bit_set(PORTC, 5) #define bind_off bit_clr(PORTC, 5) // Timer 0 macros (for pulse width count) #define timer2_start TCCR2B = 0b00000011; #define timer2_stop TCCR2B = 0b00000000; #define timer2_clear TCNT2 = 0; // "Dead-zone" constants -- to adjust the controls to "zero" #define PWM_threshold_high 127 #define PWM_threshold_low 127 #define DIR_threshold_high 125 #define DIR_threshold_low 125 // =========================================================================== // Macro Definitions - for easier code and I/O manipulation // =========================================================================== // NOTE: Volatile variables can be read/written asynchronously! // For global variables, only (initially) non-zero variables should be defined volatile unsigned char pinstate, channel; volatile unsigned char interrupts = 1; volatile unsigned char channel_pin_changes, pins_changing; volatile unsigned char channel1_toff; volatile unsigned char channel2_toff; volatile unsigned char channel3_toff; volatile unsigned char channel4_toff; volatile unsigned char channel5_toff; volatile unsigned char channel6_toff; volatile double direction_percent; volatile unsigned char channel3_value; volatile unsigned char direction_value = 0; ISR (PCINT2_vect) { pinstate = PIND; // Read in pin state ASAP pinstate &= 0b10011111; // Mask off unused bits (PORTD 6 and 7) pins_changing = 1; // "interrupts" variable stores the number of the CURRENT interrupt // referenced from the first channel's pulse going high (i.e. ch1 // from Low -> High triggeres interrupt #1) if (interrupts <= 1){ // Check if interrupt occured as a result of CH 1 pulse pin going high if (pinstate == 0b00000001) { timer2_start; interrupts = 2; } } else { // 2nd interrupt: CH. 1 just went from High -> Low if (interrupts == 2) { channel1_toff = TCNT2; // Record 'off' time stamp for CH. 1 interrupts = 3; // Increase # of interrupts counter } // 3rd interrupt: CH. 2 just went from Low -> High else if (interrupts == 3) { timer2_clear; // Zero out the timer count interrupts = 4; // Increase # of interrupts counter } // 4th interrupt: CH. 2 just went from High -> Low else if (interrupts == 4) { channel2_toff = TCNT2; // Record 'off' time stamp for CH. 2 interrupts = 5; // Increase # of interrupts counter } // 5th interrupt: CH. 3 just went from Low -> High else if (interrupts == 5) { //bit_set(PORTC, 5); timer2_clear; // Zero out the timer count interrupts = 6; // Increase # of interrupts counter } // 6th interrupt: CH. 3 just went from High -> Low else if (interrupts == 6) { //bit_clr(PORTC, 5); channel3_toff = TCNT2; // Record 'off' time stamp for CH. 3 interrupts = 7; // Increase # of interrupts counter } // 7th interrupt: CH. 4 just went from Low -> High else if (interrupts == 7) { timer2_clear; // Zero out the timer count interrupts = 8; // Increase # of interrupts counter } // 8th interrupt: CH. 4 just went from High -> Low else if (interrupts == 8) { channel4_toff = TCNT2; // Record 'off' time stamp for CH. 4 interrupts = 9; // Increase # of interrupts counter } // 9th interrupt: CH. 5 just went from Low -> High else if (interrupts == 9) { timer2_clear; // Zero out the timer count interrupts = 10; // Increase # of interrupts counter } // 10th interrupt: CH. 5 just went from High -> Low else if (interrupts == 10) { channel5_toff = TCNT2; // Record 'off' time stamp for CH. 5 interrupts = 11; // Increase # of interrupts counter } // 11th interrupt: CH. 6 just went from Low -> High else if (interrupts == 11) { timer2_clear; // Zero out the timer count interrupts = 12; // Increase # of interrupts counter } // 12th interrupt: CH. 6 just went from High -> Low else if (interrupts == 12) { channel6_toff = TCNT2; // Record 'off' time stamp for CH. 6 timer2_stop; timer2_clear; interrupts = 1; // Reset interrupts counter back to 1 } } return; } // =========================================================================== // Startup Function: This runs once after each hardware power on or reset // to define states of all the port pins, and enable peripherals for use. // =========================================================================== void startup (void) { // Port Direction Setup // Set = Output , Cleared = Input // DD registers - "Data Direction" bit_set(DDRB,0); // PORTB0 as output [ ] bit_set(DDRB,1); // PORTB1 as output [ Motor Left PWM output / LED ] bit_set(DDRB,2); // PORTB2 as output [ Motor Right PWM output / LED ] bit_set(DDRB,3); // PORTB3 as output [ Main Relay Switch ] bit_set(DDRB,4); // PORTB4 as output [ ] bit_set(DDRB,5); // PORTB5 as output [ Weapon Relay Switch and LED ] bit_set(DDRB,6); // PORTB6 as output [ ] bit_set(DDRB,7); // PORTB7 as output [ ] bit_clr(DDRC,0); // PORTC0 as input [ Battery Voltage (Analog) ] bit_set(DDRC,1); // PORTC1 as output [ LED 1 - Power LED ] bit_set(DDRC,2); // PORTC2 as output [ LED 2 - BIND LED ] bit_set(DDRC,3); // PORTC3 as output [ LED 3 - L Motor Direction output/ LED ] bit_set(DDRC,4); // PORTC4 as output [ LED 4 - R Motor Direction output/ LED ] bit_set(DDRC,5); // PORTC5 as output [ Radio Receiver BIND Pin ] bit_set(DDRC,6); // PORTC6 as output [ RESET (Unused) ] bit_clr(DDRD,0); // PORTD0 as input [ RF Channel 1 input ] bit_clr(DDRD,1); // PORTD1 as input [ RF Channel 2 input ] bit_clr(DDRD,2); // PORTD2 as input [ RF Channel 3 input ] bit_clr(DDRD,3); // PORTD3 as input [ RF Channel 4 input ] bit_clr(DDRD,4); // PORTD4 as input [ RF Channel 5 input ] bit_set(DDRD,5); // PORTD5 as output [ Motor Left Braking PWM ] bit_set(DDRD,6); // PORTD6 as output [ Motor Right Braking PWM ] bit_clr(DDRD,7); // PORTD7 as input [ RF Channel 6 input ] // Initial Port configuartion // For outputs: set = Active (pulled high) clr = Inactive (pulled low) // For inputs: set = Internal Pull-up (high) clr = No Pull-up (low) bit_clr(PORTB,0); // PORTB0 off bit_clr(PORTB,1); // PORTB1 off bit_clr(PORTB,2); // PORTB2 off bit_clr(PORTB,3); // PORTB3 off bit_clr(PORTB,4); // PORTB4 off bit_clr(PORTB,5); // PORTB5 off bit_clr(PORTB,6); // PORTB6 off bit_clr(PORTB,7); // PORTB7 off bit_clr(PORTC,0); // PORTC0 pulled low bit_clr(PORTC,1); // PORTC1 off bit_clr(PORTC,2); // PORTC2 off bit_clr(PORTC,3); // PORTC3 off bit_clr(PORTC,4); // PORTC4 off bit_clr(PORTC,5); // PORTC5 off bit_set(PORTC,6); // PORTC6 on bit_clr(PORTD,0); // PORTD0 pulled low bit_clr(PORTD,1); // PORTD1 pulled low bit_clr(PORTD,2); // PORTD2 pulled low bit_clr(PORTD,3); // PORTD3 pulled low bit_clr(PORTD,4); // PORTD4 pulled low bit_clr(PORTD,5); // PORTD5 off bit_clr(PORTD,6); // PORTD6 off bit_clr(PORTD,7); // PORTD7 pulled low // Hardware 8-bit Timer/PWM #0 setup (Braking / Regen control PWM) TCCR0A = 0b10100011; // bit 7,6 : Clear 0C0A on match, set on BOTTOM // bit 5,4 : Clear 0C0B on match, set on BOTTOM // bit 3,2 : Reserved // bit 1,0 : PWM mode, 8-Bit TOP=0xFF TCCR0B = 0b00000001; // bit 7,6 : Capture settings (ignore) // bit 5 : Reserved // bit 4,3 : Normal Timer, 8-Bit // bit 2,1,0 : Internal clock, prescale = 1 OCR0A = 0; // Start with no regen OCR0B = 0; // Start with no regen // Hardware 16-bit Timer/PWM #1 setup (Drive Motor throttle PWM) TCCR1A = 0b10100010; // bit 7,6 : Clear 0C1A on match, set on BOTTOM // bit 5,4 : Clear 0C1B on match, set on BOTTOM // bit 3,2 : Reserved // bit 1,0 : Fast PWM_main, 16-Bit TOP=ICR1 TCCR1B = 0b00011001; // bit 7,6 : Capture settings (ignore) // bit 5 : Reserved // bit 4,3 : Fast PWM_main, 16-Bit TOP=ICR1 // bit 2,1,0 : Internal clock, prescale = 1 ICR1 = 0b0000000011111111; // Sets the maximum value (8-bit mode) OCR1A = 0b0000000000000000; // Sets the PWM_main Period length/ Duty Cycle OCR1B = 0b0000000000000000; // Sets the PWM_main Period length/ Duty Cycle // Hardware 8-bit Timer/PWM #2 (Pulse Width Counter) TCCR2A = 0b00000000; // bit 7,6 : Disable 0C2A // bit 5,4 : Disable 0C2B // bit 3,2 : Reserved // bit 1,0 : Normal Timer Mode, 8-Bit TOP=0xFF TCCR2B = 0b00000100; // bit 7,6 : Capture settings (ignore) // bit 5 : Reserved // bit 4,3 : Fast PWM_main, 8-Bit TOP=0xFF // bit 2,1,0 : Internal clock, prescale = 64 // External Pin Change Interrupt Configuration - This interrupt will occur // if ANY logical change on ANY enabled PCINT pin is detected. Software // must determine which pin generated the interrupt and why. PCICR = 0b00000100; // Enable Pin change interrupts on PCINT23-16 PCMSK2 = 0b10011111; // Enable PCINTs 16 to 21 (PORTD pins 0 to 5) sei(); // Globally enable interrupts return; } void motor_pwm_apply (void) { unsigned char motor_pwm_temp = 0; // Forward (or reverse) and left if (direction_value == 0) { motor_pwm_temp = channel3_value; motor_left_pwm = 2 * motor_pwm_temp; //(exp((float)(0.04363 * motor_pwm_temp))); motor_pwm_temp = (float)(channel3_value * (float)(1.00 - direction_percent)); motor_right_pwm = 2 * motor_pwm_temp; //(exp((float)(0.04363 * motor_pwm_temp))); motor_left_braking = 0; motor_right_braking = 0; } // Forward (or reverse) and right else { motor_pwm_temp = (float)(channel3_value * (float)(1.00 - direction_percent)); motor_left_pwm = 2 * motor_pwm_temp; //(exp((float)(0.04363 * motor_pwm_temp))); motor_pwm_temp = channel3_value; motor_right_pwm = 2 * motor_pwm_temp; //(exp((float)(0.04363 * motor_pwm_temp))); motor_left_braking = 0; motor_right_braking = 0; } // Turn on Regen braking when both motors are not being throttled... if ((motor_left_pwm < 6) && (motor_right_pwm < 6)) { motor_left_braking = 127; motor_right_braking = 127; } return; } int main (void) { unsigned char channel4_value = 0, channel3_sign = 0; unsigned int dcount = 0; // delay counter // Perform inital (one-time) startup tasks startup(); relay_main_off; led_power_on; // Radio Receiver BINDing routine // NOTE: Runs continuously as long as the radio receiver is NOT bound // to the radio transmitter (remote controller). Exits when BINDing is done. while (1) { bind_off; motor_left_pwm = 0; motor_right_pwm = 0; motor_weapon_off; relay_main_off; // Set BIND pin high and exit when inputs become active if (pins_changing != 0) { bind_on; break; } // Bind Blink routine (simple: blinking = still binding) if (dcount >= 9500) { led_bind_tog; dcount = 0; } dcount++; } // This loop should run continuously until the Microcontroller is powered-down. while (1) { // BIND LED is lit solid during normal operation and main relay engaged. led_bind_on; relay_main_on; if (channel1_toff > 220) { motor_right_pwm = 255; motor_right_for; motor_left_pwm = 255; motor_left_rev; } else if (channel1_toff < 30) { motor_right_pwm = 255; motor_right_rev; motor_left_pwm = 255; motor_left_for; } else { // LEFT <-> RIGHT directional Logic (Channel 4) // Direction: Right if (channel4_toff > DIR_threshold_high){ channel4_value = (channel4_toff - 125); direction_value = 1; direction_percent = (float)channel4_value * 0.0078125; //125; } // Direction: Left else if (channel4_toff < DIR_threshold_low) { channel4_value = (125 - channel4_toff); direction_value = 0; direction_percent = (float)channel4_value * 0.0078125; //512; } // Direction: Neutral else { channel4_value = 0; direction_value = 1; direction_percent = 0.00; } // Motor FORWARD <-> REVERSE (Channel 3) Logic // Direction: Forward if (channel3_toff > PWM_threshold_high){ motor_right_for; motor_left_for; channel3_sign = 1; channel3_value = (channel3_toff - 127); //125 motor_pwm_apply(); } // Direction: REVERSE else if (channel3_toff < PWM_threshold_low) { motor_right_rev; motor_left_rev; channel3_sign = 0; channel3_value = (127 - channel3_toff); //125 motor_pwm_apply(); } // Direction: STOPPED else { motor_right_for; motor_left_for; channel3_sign = 1; channel3_value = 0; motor_pwm_apply(); } } // WEAPON motor (Channel 5) Logic if (channel5_toff >= 127) motor_weapon_on; else if (channel5_toff < 127) motor_weapon_off; channel_pin_changes = 0; } return (0); // This should never ever return!!!! }