/*****************************************************************************
*
* Copyright (c) 2013
www.der-frickler.net
*
*
* This library is free software; you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published
* by the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This software is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*****************************************************************************
*
* ESC Light Module
* Uses an Hobbyking 6A UBEC Brushless ESC as light switch.
* switches the different lights on dependent to the stick position .
*
* PINOUT: Servo Signal IN: INT0 (PD2)
* OUT1 RED WIRE
* OUT2 BLUE WIRE
* OUT3 BLACK Wire
*
*****************************************************************************/
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#define IN PD2
#define STICK_LOW 200
#define STICK_CENTER 600
#define STICK_HIGH 900
#define STICK_OVERFLOW 4000
#define STROBE_DIST 500
#define STROBE_TIME 20
#define STROBE_REPEAT 180
#define ACL_DIST 280
#define ACL_TIME 20
static void setupPorts(void) {
DDRB = 255; // portb as output
PORTB = 0; // portb to 0
DDRC = 255; // portc as output
PORTC = 0; // portc to 0
DDRD = 48; // portd as output, int0 as input
PORTD = 0; // portd to 0
}
// for PFET = switched VCC
static void out1_on(void) { PORTD |= (1 << 4); }
static void out1_off(void) { PORTD &= ~(1 << 4); }
static void out2_on(void) { PORTC |= (1 << 5); }
static void out2_off(void) { PORTC &= ~(1 << 5); }
static void out3_on(void) { PORTC |= (1 << 3); }
static void out3_off(void) { PORTC &= ~(1 << 3); }
// // for NFET = switched GND
// static void out1_on(void) { PORTD |= (1 << 5); }
// static void out1_off(void) { PORTD &= ~(1 << 5); }
// static void out2_on(void) { PORTC |= (1 << 4); }
// static void out2_off(void) { PORTC &= ~(1 << 4); }
// static void out3_on(void) { PORTB |= (1 << 0); }
// static void out3_off(void) { PORTB &= ~(1 << 0); }
uint8_t color[3];
uint8_t i = 0;
uint16_t ppm = 0; // measures PPM frame length
uint16_t fade = 0;
uint16_t fader = 1;
// on timer1 overflow
ISR (TIMER1_OVF_vect)
{
ppm = STICK_OVERFLOW;
}
// on INT0 / PD2 pinchange
ISR (INT0_vect)
{
if ((PIND & 4)==4) // check 3th bit of PORTD (pin 4) got HIGH
{
//TCCR1B = 0; // stop timer1
TCNT1 = 0; // reset timer1
TCCR1B = 1; // start 16bit timer1 - no prescale
}
else if ((PIND & 4)==0 )
{ // check 3th bit of PORTD (pin 4) got LOW
TCCR1B = 0; // stop timer1
uint16_t value = TCNT1; // get Timer Value
// map measured value to 0-255
if (value > 32000) {
ppm = 1000;
}else if (value < 16000) {
ppm = 0;
} else {
ppm = ((value - 16000) / 16);
}
}
}
ISR (TIMER0_OVF_vect) // just constant timebase for PWM generation
{
// Interrupt action every (16000000)/256 Hz = 62,5 kHz
// leads to 255steps pwm with ~ 245 Hz
// if (i == 0) {
// out1_on();
// out2_on();
// out3_on();
// }
// if (i == color[0]) out1_off();
// if (i == color[1]) out2_off();
// if (i == color[2]) out3_off();
// i++;
// if (i > 255) i = 0;
}
int main (void)
{
setupPorts();
MCUCR = 1; // External Interrupt on PinChange
GICR = 64; // enable Int0
// Timer 1 - measures input signal
TCCR1A = 0; // 16-Bit Normal Mode
TIMSK |= (1<<TOIE1); // Timer Overflow-Interrupt an
TCCR1B = 0; // stop 16-Bit Timer
TCNT1 = 0; // reset timer1
// Timer 0 - timebase for pwm
TCCR0 = (1<<CS00); // 8-bit Timer 0 without Prescaler = 16mhz
TIMSK |= (1<<TOIE0); // enable overflow Interrupt
sei(); // Interrupts on.
TCCR1B = 1; // start timer1
out1_off();
out2_off();
out3_off();
while(1)
{
// // LANDING LIGHT @ FULL STICK
// if (ppm > STICK_HIGH) {
// out1_on();
// } else {
// out1_off();
// }
// STROBE AND ACL @ STICK CENTER
// if (ppm > STICK_CENTER) {
// _delay_ms(STROBE_DIST);
// out2_on(); // flash strobe on
// //out1_on();
// _delay_ms(STROBE_TIME);
// out2_off(); // flash strobe off
// //out1_off();
// _delay_ms(STROBE_REPEAT);
// out2_on(); // flash strobe on
// _delay_ms(STROBE_TIME);
// out2_off(); // flash strobe off
//
// _delay_ms(ACL_DIST);
// out3_on(); // flash ACL on
// _delay_ms(ACL_TIME);
// out3_off(); // flash ACL off
// }
// // POLICE LIGHT @ STICK CENTER
// if (ppm > STICK_CENTER) {
// _delay_ms(STROBE_DIST);
// out2_on(); // left light on
// _delay_ms(STROBE_TIME);
// out2_off();
// _delay_ms(STROBE_REPEAT);
// out2_on();
// _delay_ms(STROBE_TIME);
// out2_off();
// _delay_ms(STROBE_DIST);
// out3_on(); // left light on
// _delay_ms(STROBE_TIME);
// out3_off();
// _delay_ms(STROBE_REPEAT);
// out3_on();
// _delay_ms(STROBE_TIME);
// out3_off();
// }
// // Running Light
if (ppm > STICK_LOW) {
_delay_ms(ppm/3);
out1_on(); // left light on
out3_off(); // right light off
_delay_ms(ppm/3);
out1_off(); // left light off
out2_on(); //right light on
_delay_ms(ppm/3);
out2_off(); // left light off
out3_on(); //right light on
}
// // ALL OFF @ STICK LOW
if (ppm <= STICK_LOW) {
out1_off();
out2_off();
out3_off();
}
// RGB Led PWM - autofading with stop
// if (ppm < 900) fade++;
// hsb2rgbAN1(fade, 255, 255, color);
// if (fade > 767) fade = 0;
// _delay_ms(ppm/100);
// // RGB Led PWM - stick-fading
// if (ppm > 767) ppm = 767;
// hsb2rgbAN1(ppm, 255, 255, color);
// _delay_ms(10);
}
}
void hsb2rgbAN1(uint16_t index, uint8_t sat, uint8_t bright, uint8_t color[3]) {
uint8_t temp[5], n = (index >> 8) % 3;
temp[0] = temp[3] = (uint8_t)(( (sat ^ 255) * bright) / 255);
temp[1] = temp[4] = (uint8_t)((((( (index & 255) * sat) / 255) + (sat ^ 255)) * bright) / 255);
temp[2] = (uint8_t)(((((((index & 255) ^ 255) * sat) / 255) + (sat ^ 255)) * bright) / 255);
color[0] = temp[n + 2];
color[1] = temp[n + 1];
color[2] = temp[n ];
}