/*

	MOTORS.CC

	Motor class controls the motors installd on the balancing
	robot, Ballybot.

	Alistair Sutherland
	suthe-aj@ee.uwa.edu.au
	CIIPS, 2003

*/

#include <stdlib.h>
#include "motors.h"

CMotors::CMotors()
{
	m_motorLeft  = MOTORInit(MOTOR_LEFT);
	m_motorRight = MOTORInit(MOTOR_RIGHT);
	if(m_motorLeft == 0 || m_motorRight == 0)
	{
		LCDPrintf("Motor Error!\n");
		OSWait(200); 
	}
	
	// Init QUADS
	m_quadLeft = QUADInit(QUAD_LEFT);
	m_quadRight = QUADInit(QUAD_RIGHT);

	// Reset motor...
	Reset();
}

CMotors::~CMotors()
{
	// Switch motors off...
	MOTORDrive( m_motorLeft, 0 );
	MOTORDrive( m_motorRight, 0);

	// Release quadrature-decoders
	QUADRelease( m_quadLeft );
	QUADRelease( m_quadRight );

	// Release motors
	MOTORRelease(m_motorLeft);
	MOTORRelease(m_motorRight);
}

int CMotors::Reset()
{
	// Turn motor off...
	MOTORDrive( m_motorLeft, 0 );
	MOTORDrive( m_motorRight, 0);

	// Initialise Quadrature decoders
	QUADODOReset(QUAD_LEFT);
	QUADODOReset(QUAD_RIGHT);

	m_fLastError = 0.0;
	m_fIntegratedError = 0.0;
	m_iPIDCounter = 2;
	m_fCorrection = 0.0;
	m_fLeftDistance = 0.0;
	m_fRightDistance = 0.0;

	return MOTOR_OK;
}

int CMotors::SetForce(float fForce, float fVelocity)
{
	float fError;
	float fDiffError;
	float fForceRight, fForceLeft;
	float fVoltsRight, fVoltsLeft;

	// Determine friction
	float fFriction;
	if (fVelocity>0.0001) fFriction = -FRICTION_FORCE;
	if (fVelocity<-0.0001) fFriction = FRICTION_FORCE;
	fForce -= fFriction;

	m_iPIDCounter--;
	if (m_iPIDCounter < 1)
	{
		m_iPIDCounter =2;
		
		// Determine error in motors:
		fError = (m_fLeftDistance - m_fRightDistance);
		m_fIntegratedError += fError;
		fDiffError = fError - m_fLastError;
		m_fLastError = fError;
		m_fCorrection = P_ * fError + I_ * m_fIntegratedError + D_ * fDiffError;
	}
		
	fForceLeft  = fForce - m_fCorrection;
	fForceRight = fForce + m_fCorrection;

	// Calculate voltage to produce this force...
	fVoltsLeft = (2.58 * fForceLeft) / (0.0805 - 0.035 * fVelocity);
	fVoltsRight = (2.58 * fForceRight) / (0.0805 - 0.035 * fVelocity);

	// Limit fVoltage to +- 100%
	int iVoltsLeft, iVoltsRight;
	iVoltsLeft = (int)fVoltsLeft; iVoltsRight = (int)fVoltsRight;
	if (iVoltsLeft < -100) iVoltsLeft = -100;
	if (iVoltsLeft > 100)  iVoltsLeft = 100;
	if (iVoltsRight < -100) iVoltsRight = -100;
	if (iVoltsRight > 100)  iVoltsRight = 100;

	// Apply voltage to motors:
	MOTORDrive( m_motorLeft, iVoltsLeft  );
	MOTORDrive( m_motorRight, iVoltsRight );

	return MOTOR_OK;
}

float CMotors::GetDistance()
{
	m_fLeftDistance = QUADODORead( m_quadLeft );
	m_fRightDistance = QUADODORead( m_quadRight );
	return ( (m_fLeftDistance + m_fRightDistance) / 2.0 );
}