void SetThrust(void);
unsigned int GetMotorPW(char thrust_pgain, char thrust_dgain, int error, int prevError);

int prevThrustError = 0;

//-----------------------------------------------------------------------------------------
// Set Motor Thrust
//-----------------------------------------------------------------------------------------

void SetThrust(void) {
	int range, thrustError, rangerPulsewidth;

	range = readRanger();
	thrustError = 150 - range;
	rangerPulsewidth = GetMotorPW(Cpn, Cdn, thrustError, prevThrustError);
	prevThrustError = thrustError;

	PCA0CPL2 = 0xFFFF - rangerPulsewidth;
	PCA0CPH2 = (0xFFFF - rangerPulsewidth) >> 8;
	PCA0CPL3 = 0xFFFF - rangerPulsewidth;
	PCA0CPH3 = (0xFFFF - rangerPulsewidth) >> 8;
}

//-----------------------------------------------------------------------------------------
// Find Motor Pulsewidth
//-----------------------------------------------------------------------------------------

unsigned int GetMotorPW(char thrust_pgain, char thrust_dgain, int error, int prevThrustError)
{
	int pulsewidth;

	pulsewidth = THRUST_NEUT + thrust_pgain * error + thrust_dgain * (error - prevThrustError);

	if(pulsewidth > THRUST_MAX)
		pulsewidth = THRUST_MAX;
	else if(pulsewidth < THRUST_MIN)
		pulsewidth = THRUST_MIN;


	return (unsigned int)pulsewidth;
}