AVR Freaks

const float OVF_IN_1_SEC = F_CPU/256;
const float SERVO_MIN = 1.0; // ms
const float SERVO_MIN_IN_OVF = SERVO_MIN * OVF_IN_1_SEC / 1000;
const float SERVO_NEUTRAL = 1.5; // ms
const float SERVO_MAX = 2.0; // ms
const float SERVO_MAX_IN_OVF = SERVO_MAX * OVF_IN_1_SEC / 1000;

const float PRECISION = (float)1000 / ( (float)F_CPU / 256 ); // 1 s = 1000 ms; F_CPU / 256 = how many times it overflows per second; PRECISION tells you how much time passed since last overflow :)
const float SERVO_FREQUENCY = 20; // ms
const float SERVO_FREQUENCY_IN_OVF = SERVO_FREQUENCY * OVF_IN_1_SEC / 1000;

void printInfo()
{
	cout << "F_CPU = " << F_CPU << endl;
	cout << "OVF_IN_1_SEC = " << OVF_IN_1_SEC << endl;
	cout << "SERVO_MIN = " << SERVO_MIN << endl;
	cout << "SERVO_MIN_IN_OVF = " << SERVO_MIN_IN_OVF << endl;
	cout << "SERVO_NEUTRAL = " << SERVO_NEUTRAL << endl;
	cout << "SERVO_MAX = " << SERVO_MAX << endl;
	cout << "SERVO_MAX_IN_OVF = " << SERVO_MAX_IN_OVF << endl;
	cout << "PRECISION = " << PRECISION << endl;
	cout << "SERVO_FREQUENCY = " << SERVO_FREQUENCY << endl;
	cout << "SERVO_FREQUENCY_IN_OVF = " << SERVO_FREQUENCY_IN_OVF << endl;
}

class Object
{
public:
	inline Object() : m_duty(SERVO_NEUTRAL), m_port(0), m_pin(0), m_expired(1) { }
	inline ~Object() { }
	inline void setDuty( float duty ) { if(duty >= SERVO_MIN && duty <= SERVO_MAX) m_duty = duty; else { m_duty = 0; setLow(); } }
	inline float getDuty() { return m_duty; }
	inline void setExpired( volatile uint8_t expired ) { m_expired = expired; }
	inline uint8_t isExpired() { return m_expired; }
	inline void setOutput( volatile uint8_t *port, uint8_t pin )
	{
		m_port = port;
		m_pin = pin;
	}
	inline void setHigh() { bit_set(*m_port, BIT(m_pin)); }
	inline void setLow() { bit_clear(*m_port, BIT(m_pin)); }

private:
	float volatile m_duty; // in ms
	volatile uint8_t *m_port;
	volatile uint8_t m_pin;
	volatile uint8_t m_expired;
};

class ObjectsVector
{
public:
	inline ObjectsVector() : m_list(0), m_length(0), m_maxLength(0) { }
	inline ~ObjectsVector()
	{
		if(m_list)
			free(m_list);
	}
	inline void init( uint8_t maxLength )
	{
		m_maxLength = maxLength;
		if( (m_list = (Object**)malloc( m_maxLength * sizeof(Object) )) == NULL )
			m_maxLength = 0;
	}
	
	
	inline uint8_t add( Object *object )
	{
		if( m_length < m_maxLength )
		{
			m_list[m_length++] = object;
			sort();
			return FALSE;
		}
		else
			return TRUE;
	}

	/*inline Object *getHead() { return m_list[0]; }
	inline Object *getTail() { return m_list[m_length]; }*/
	inline Object **getList() { return m_list; }
	inline uint8_t getLength() { return m_length; }
	inline void sort()
	{
		if(m_length == 1)
			return;
		for(i = 0; i < m_length - 1; ++i)
			for(uint8_t j = i + 1; j < m_length; ++j)
			{
				if ( this->getList()[i]->getDuty() < this->getList()[j]->getDuty() )
				{
					Object * temp = this->getList()[i];
					this->getList()[i] = this->getList()[j];
					this->getList()[j] = temp;
				}
			}
	}

private:
	Object **m_list;
	uint8_t m_length; // m_index-1 is the last one added ;)
	uint8_t m_maxLength; // max length
	
};
void softwarePWM();
extern ObjectsVector myList;

// the list must be sorted from the highest duty to the lowest

ObjectsVector myList;
//todo: put the ++Timer0_OVF_Counter at the very beginning of the softwarePWM then compare everything to value+1
void softwarePWM()
{
	static float time_passed = 0;
	static uint16_t Timer0_OVF_Counter = 0;

	if(Timer0_OVF_Counter == 0) // we are starting again :)
	{
		for(i = 0; i < myList.getLength(); ++i)
			if ( myList.getList()[i]->getDuty() )
			{
				myList.getList()[i]->setHigh();
				myList.getList()[i]->setExpired(0);				
			}
		//return;
	}

	//if(Timer0_OVF_Counter >= 62 && Timer0_OVF_Counter < 125)
	if(Timer0_OVF_Counter >= SERVO_MIN_IN_OVF && Timer0_OVF_Counter <= SERVO_MAX_IN_OVF) // ONLY if we are between SERVO_MIN - SERVO_MAX, there is no point to check SERVO_MAX - SERVO_FREQUENCY ms, everyone should be "sleeping" already :)
	{
		//time_passed = Timer0_OVF_Counter * 0.016;
		time_passed = Timer0_OVF_Counter * PRECISION;
		for(i = 0; i < myList.getLength(); ++i)
		{
			if( !myList.getList()[i]->getDuty() )
				continue;
			else if( myList.getList()[i]->isExpired() )
				break; // because our list is ordered from the highest to the lowest, if we found one that is expired, everyone after him should be expired
			else if( time_passed >= myList.getList()[i]->getDuty() )
			{
				myList.getList()[i]->setLow();
				myList.getList()[i]->setExpired(TRUE);
			}
		}
		//return;
	}

//	if(Timer0_OVF_Counter == 1250) // 20 ms passed! reset :)
	if(Timer0_OVF_Counter == SERVO_FREQUENCY_IN_OVF) // SERVO_FREQUENCY ms passed! reset :)
		Timer0_OVF_Counter = 0;
	else
		++Timer0_OVF_Counter;
}

SIGNAL( TIMER0_OVF_vect )
{
	if( Timer0::_timer0OvfHandler )
		Timer0::_timer0OvfHandler();
}

int main()
{
    DDRB = 0b11101110; //PB4 = MISO 
    DDRC = 0b11111110; //
    DDRD = 0b11110000; //PORTD (RX on PD0)

	//wdt_enable( WDTO_1S );

	//adcInit();
	//adcSetPrescaler(ADC_PRESCALE_DIV128);

	UART::Init( 0, 9600 );

	unsigned float duty = 1.5f;

	Object led;
	led.setDuty( duty );
	led.setOutput( &PORTC, 1 );
	myList.init( 3 );
	myList.add( &led );

	Timer0::init( TIMER0_CLK_DIV1 );
	Timer0::attachHandler( softwarePWM );

	uint8_t c = 0;

	//wdt_reset();

	for( ;; ) {
		//wdt_reset();
		/*if( adcConvert10bit(0) < 500 )
			bit_set(PORTC, BIT(1));
		else
			bit_clear(PORTC, BIT(1));*/
		bit_set( PORTB, BIT(1) );
		if( (c = UART::getByte()) )
		{
			(void)printf( "code = %c\n", c);
			/*if(c == '1')
			{
				(void)printf( "ADCH = %i\n", adcConvert10bit(0) );
			}*/
			if( c == '2' )
			{
				duty += 0.1f;
				led.setDuty( duty );
			}
			else if( c == '3' )
			{
				duty -= 0.1f;
				led.setDuty( duty );
			}
			else if( c == '4' )
			{
				led.setDuty( 0.0f );
			}

			UART::sync();
		}
	}
	
	return 0;
}