File content as of revision 23:797d25f3df5e:

#include "PID_controller.h"

PIDC::PIDC() :
    offSetDegree(0),
    turnOverNumber(0),
    beforeDegree(0),
    rawDegree(0),
    calculationResult(0),
    currentDegree(0)
{
    pid = new PID(KC, TI, TD, INTERVAL);
    pid -> setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
    pid -> setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
    pid -> setBias(BIAS);
    pid -> setMode(AUTO_MODE);
    pid -> setSetPoint(0.0);

    wait(0.1);
    hmc -> setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(0.1);
    rawDegree = hmc -> sample();
    beforeDegree = rawDegree;
    offSetDegree = rawDegree;
}

PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
    offSetDegree(0),
    turnOverNumber(0),
    beforeDegree(0),
    rawDegree(0),
    calculationResult(0),
    currentDegree(0)
{
    pid = new PID(kc, ti, td, interval);
    hmc = new HMC6352(sda, scl);
    pid -> setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
    pid -> setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
    pid -> setBias(BIAS);
    pid -> setMode(AUTO_MODE);
    pid -> setSetPoint(0.0);

    wait(0.1);
    hmc -> setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(0.1);
    rawDegree = hmc -> sample();
    beforeDegree = rawDegree;
    offSetDegree = rawDegree;
}


void PIDC::confirm()
{
    rawDegree = hmc -> sample();
    if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
    if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
    currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
    beforeDegree = hmc -> sample();
    pid -> setProcessValue(currentDegree / 10.0);
    calculationResult = pid -> compute();
}

void PIDC::setPoint(float point)
{
    pid -> setSetPoint(point);
}

float PIDC::getCalculationResult() const
{
    return calculationResult;
}

int PIDC::getCurrentDegree() const
{
    return currentDegree;
}

int PIDC::getRawDegree()
{
    return hmc -> sample();
}

void PIDC::resetOffset()
{
    beforeDegree = hmc -> sample();
    offSetDegree = hmc -> sample();
    turnOverNumber = 0;
}

void PIDC::calibration(int mode)
{
    hmc -> setCalibrationMode(mode);
}