File content as of revision 0:2396598427bf:

#include "headingHold.h"


headingHold::headingHold(double p,double i,double d) :
 headingPID(p,i,d,0.01)
{
  headingPID.setInputLimits(-180,180);
  headingPID.setOutputLimits(-1,1);
  headingPID.setBias(0.0);
  headingPID.setMode(AUTO_MODE);
  targetHeading = 0;
  holding = false;
}

bool headingHold::setHeadingHold(const bool& hold)
{
  if(holding == false)
  {
    targetHeading = currentHeading;
  }
  if(holding == hold)
    return true;
  holding = hold;
  if(holding)
  {
    headingPID.reset();
  }
  return 0;
}
bool headingHold::setCurrentHeading(const double& heading){
  if (fabs(heading) >180)
  {
    return 1;
  }
  currentHeading = heading;
  return 0;
}

bool headingHold::setTargetHeading(const double& target)
{
  if (fabs(target) >180)
  {
    return 1;
  }
  targetHeading = target;
  return 0;
}

double headingHold::getOutput()
{
  double error = targetHeading-currentHeading;
  if(error < -180.0) error += 360;
  else if(error > 180.0) error -= 360;

  headingPID.setProcessValue(error);
  return headingPID.compute();
}