/*
  L298TestTask -- run the L298 using Schip Task scheduler
  Turns on H-bridge for RUNTIME, then off for OFFTIME,
   then reverses direction, repeatedly.
  Also sequences each transistor switch on same period
 
  This example code is in the public domain.
 */
#include "L298.h"

// get the prototypes for the schip-scheduler
//  (hidden in arduino-1.0.4/hardware/arduino/cores/schip
//   and accessed from default cores/arduino directory)
#include "schip/scheduler.h"

// milli-seconds between speeds
#define TICKS	1000
#define SPEEDS	5	// number of speed settings, plus 0=OFF
					//  note that 5 is a magic number,
					//   the last step is 255 or full on

#define SPDSTEP	(255/SPEEDS)

L298 myL298;	// instantiate the L298 and set all pins correctly
#ifdef HBSWboard
Trsw myTrsw;	// instantiate the transistor switched and set pins correctly
#endif // HBSWboard

/** task executed to run h-bridge around
 *  reposts itself over and over with new states and speeds
 *  @param state -- statemachine value as defined below
 **/
// current state of system
#define STATE0	0	// starting up
#define RunFwd	1	// running forward
#define StopFwd	2	// stop after forward
#define RunBkd	3	// running backward
#define StopBkd	4	// stop after backward

void L298TestTask( uint16_t state )
{
	static byte speed;
	byte s;

	// alternate LED on/off
	digitalWrite( LED, state&0x01 );

	switch( state )
	{
		case STATE0:	// startup
			speed = 0;
			state = RunFwd;
			// fall through
		case RunFwd:
			if( speed == 0 )
			{
				myL298.stop( 0 );
				myL298.stop( 1 );
			}
			else
			{
				s = speed*SPDSTEP;
				myL298.runFwd( 0, s );
				myL298.runFwd( 1, s );
			}

			if( ++speed > SPEEDS )
			{
				speed = 0;
				state = RunBkd;
			}
		break;

		case RunBkd:
			if( speed == 0 )
			{
				myL298.stop( 0 );
				myL298.stop( 1 );
			}
			else
			{
				s = speed*SPDSTEP;
				myL298.runBkd( 0, s );
				myL298.runBkd( 1, s );
			}

			if( ++speed > SPEEDS )
			{
				speed = 0;
				state = RunFwd;
			}
		break;

		default: // oy...
		break;
	}

#ifdef HBSWboard
	// use speed to sequence through transistor switches
	switch( speed )
	{
		case 1:
			  digitalWrite( TRSWA, HIGH);
		break;

		case 2:
			  digitalWrite( TRSWA, LOW);
			  digitalWrite( TRSWB, HIGH);
		break;

		case 3:
			  digitalWrite( TRSWB, LOW);
			  digitalWrite( TRSWC, HIGH);
		break;

		case 4:
			  digitalWrite( TRSWC, LOW);
			  digitalWrite( TRSWD, HIGH);
		break;

		case 0:
		default:
			  digitalWrite( TRSWD, LOW);
		break;
	}
#endif // HBSWboard

	repostTask( TICKS, state );
	return;
}