// oNMD_Motors.osc
//
// Routines for driving a pair of 4DQ NCC-35 motor driver boards
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
// Contributors:
//   Neil Durant <neil@octrix.com>
//
// This user class encapsulates functionality for driving a pair of
// 4QD NCC-35 motor driver baords, intended for use on a robot with
// differential drive.  For each board you can control its speed, and direction
// separately.  Ignition for the two motors is linked with an oFanout object,
// to ensure that the two motors star at almost exactly the same time.

// Public
oBit ignition			= new oBit;
oPWM mot1Sp				= new oPWM;
oPWM mot2Sp				= new oPWM;

// Private

oDio1 mot1Ig			= new oDio1;
oDio1 mot1Rv			= new oDio1;

oDio1 mot2Ig			= new oDio1;
oDio1 mot2Rv			= new oDio1;

oFanout fanIgnition		= new oFanout(2);

// Constants

const kMot1Ig	= 11;	// Motor 1 ignition on I/O line 11
const kMot2Ig	= 9;	// Motor 2 ignition on I/O line 9
const kMot1Sp	= 1;	// Motor 1 PWM on I/O line 17
const kMot2Sp	= 0;	// Motor 2 PWM on I/O line 18
const kMot1Rv	= 10;	// Motor 1 reverse on I/O line 10
const kMot2Rv	= 8;	// Motor 2 reverse on I/O line 8

Sub void main(void)
{
	/////////////////////////////////////////////////////////
	// Set up ignition
	/////////////////////////////////////////////////////////

	ignition = cvOff;
	
	mot1Ig.IOLine = kMot1Ig;
	mot1Ig.Direction = cvOutput;
	mot1Ig.Value = cvHigh;

	mot2Ig.IOLine = kMot2Ig;
	mot2Ig.Direction = cvOutput;
	mot2Ig.Value = cvHigh;
	
	fanIgnition.Input.Link(ignition);
	fanIgnition.Output1.Link(mot1Ig);
	fanIgnition.Output2.Link(mot2Ig);
	fanIgnition.Operate = cvTrue;

	/////////////////////////////////////////////////////////
	// Set up speed
	/////////////////////////////////////////////////////////

	mot1Sp.IOLine = kMot1Sp;
	mot1Sp.Period = 156;
	mot1Sp.Prescale = 2;
	mot1Sp = 0;
	mot1Sp.Operate = cvTrue;

	mot2Sp.IOLine = kMot2Sp;
	mot2Sp.Period = 156;
	mot2Sp.Prescale = 2;
	mot2Sp = 0;
	mot2Sp.Operate = cvTrue;

	/////////////////////////////////////////////////////////
	// Set up reverse
	/////////////////////////////////////////////////////////
	
	mot1Rv.IOLine = kMot1Rv;
	mot1Rv.Direction = cvOutput;
	mot1Rv.Value = cvLow;

	mot2Rv.IOLine = kMot2Rv;
	mot2Rv.Direction = cvOutput;
	mot2Rv.Value = cvLow;
}

////////////////////////////////////////////////////////////////////////////////////////
// Public methods
////////////////////////////////////////////////////////////////////////////////////////

Sub void SetForwards(void)
{
	mot1Rv = cvHigh;
	mot2Rv = cvHigh;
}

Sub void SetBackwards(void)
{
	mot1Rv = cvLow;
	mot2Rv = cvLow;
}

Sub void SetRotateLeft(void)
{
	mot1Rv = cvHigh;
	mot2Rv = cvLow;
}

Sub void SetRotateRight(void)
{
	mot1Rv = cvLow;
	mot2Rv = cvHigh;
}

Sub void Stop(void)
{
	ignition = cvOff;
}

Sub void Start(void)
{
	ignition = cvOn;
}

// End of oNMD_Motors.osc