/elec/quadcopter

//

// main.ino

//

// Testing controlling ESCs (and motors).

#include <limits.h>

// number of ESCs

#define NUM_CHANNELS 4

// pin number of first channel

#define FIRST_PIN 4

// 0 <= channel value <= this constant

#define MAX_CHANNEL_VALUE 1000UL

// pulse width when channel value is 0 and MAX_CHANNEL_VALUE in microseconds

#define MIN_PULSE_WIDTH 1150UL

#define MAX_PULSE_WIDTH 1750UL

// interval between rising edges of successinve channels in microseconds

#define CHANNEL_INTERVAL ( MAX_PULSE_WIDTH + 500UL )

// duration of a whole frame (including the pulse) in microseconds

#define FRAME_DURATION 20000UL

// test ramping

#define RAMP_START 0

#define RAMP_MAX 900

// deltas (applied every 100 milliseconds)

#define RAMP_UP_DELTA 4

#define RAMP_DOWN_DELTA -10

void setup()

{

	// setup outputs

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

		pinMode( pin, OUTPUT );

		digitalWrite( pin, LOW );

	}	

	Serial.begin( 9600 );

}

signed long calculate_duration( unsigned long then, unsigned long now )

{

	// does it look like now has overflowed (and wrapped)?

	if( now < then && now < ( ULONG_MAX / 2 ) && then > ( ULONG_MAX / 2 ) )

		return now + ( ULONG_MAX - then );

	// else, calculate duration

	else

		return now - then;

}

void update_channels( int channels[] )

{

	static int event = NUM_CHANNELS * 2;

	unsigned long now = micros();

	static unsigned long frame_start = now - FRAME_DURATION;

	static unsigned long next_event_at;

	if( now >= next_event_at )

	{

		// action event

		digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );

		// move to next event

		if( ++event >= NUM_CHANNELS * 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 );

		}

	}

}

void loop()

{

	int channels[ NUM_CHANNELS ];

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

		channels[ a ] = 0;

	// test ramping state

	unsigned long then = millis();

	int val = RAMP_START, delta = RAMP_UP_DELTA;

	while( true )

	{

		update_channels( channels );

		// update ramp value and set channel 0 value

		unsigned long now = millis();

		if( now > then + 100 ) {

			then = now;

			val += delta;

			if( val >= RAMP_MAX ) {

				val = RAMP_MAX;

				delta = RAMP_DOWN_DELTA;

			}

			if( val <= 0 ) {

				val = 0;

				delta = 0;

			}

			channels[ 0 ] = val;

		}

	}

}