/elec/quadcopter

//

// motor.cc

//

#include "motors.h"

#include "config.h"

#include "common.h"

#include <Arduino.h>

#include <limits.h>

static int channels_[ NUM_MOTORS ];

void Motors::setup()

{

	// setup outputs

	for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_MOTORS; pin++ ) {

		pinMode( pin, OUTPUT );

		digitalWrite( pin, LOW );

	}

	// reset channel values

	for( int a = 0; a < NUM_MOTORS; a++ )

		channels_[ a ] = 0;

}

void Motors::set_values( int channel_values[] )

{

	for( int a = 0; a < NUM_MOTORS; a++ )

		channels_[ a ] = channel_values[ a ];

}

void Motors::update()

{

	static int event = NUM_MOTORS * 2;

	unsigned long now = micros();

	static unsigned long frame_start = now - FRAME_DURATION;

	static unsigned long next_event_at = 0;

	if( now >= next_event_at )

	{

		// action event

		if( event < NUM_MOTORS * 2 )

			digitalWrite( FIRST_PIN + ( event / 2 ),

				( event & 1 )? LOW : HIGH );

		// move to next event

		if( ++event >= NUM_MOTORS * 2 ) {

			event = 0;

			frame_start += FRAME_DURATION;

		}

		// calculate the time that the next event will occur

		next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;

		if( event & 1 )

			next_event_at += MIN_PULSE_WIDTH + channels_[ event / 2 ] *

				( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;

		// Serial.print( event / 2 );

		// Serial.print( ( event & 1 )? 'v' : '^' );

		// Serial.print( " " );

		// Serial.println( next_event_at );

		// if( event == 1 ) {

		// 	unsigned long width = MIN_PULSE_WIDTH +

		// 		(unsigned long)channels_[ event / 2 ] *

		// 		( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;

		// 	Serial.print( channels_[ event / 2 ] );

		// 	Serial.print( " " );

		// 	Serial.println( width );

		// }

	}

	// else, if an event is coming up soon, sleep and service the event to make

	// sure that no other code runs and blocks us!

	else

	{

		long delay = calculate_duration( now, next_event_at );

		if( delay < 250 )

		{

			if( delay > 20 )

				delayMicroseconds( delay - 20 );

			update();

		}

	}

}