File content as of revision 51:794f160b09d4:

#include "PID_controller.h"

PIDC::PIDC() :
    pid(KC, TI, TD, INTERVAL),
    r1370(PC_6, PC_7),
    pidcSerial(USBTX, USBRX, 115200),
    offSetDegree(0),
    turnOverNumber(0),
    beforeDegree(0),
    rawDegree(0),
    calculationResult(0),
    currentDegree(0)
{
    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);
    r1370.update();
    wait(0.1);
    rawDegree = r1370.getRate();
    beforeDegree = rawDegree;
    offSetDegree = rawDegree;
}

PIDC::PIDC(PinName tx, PinName rx, float kc, float ti, float td, float interval) :
    pid(kc, ti, td, interval),
    r1370(tx, rx),
    pidcSerial(USBTX, USBRX, 115200),
    offSetDegree(0),
    turnOverNumber(0),
    beforeDegree(0),
    rawDegree(0),
    calculationResult(0),
    currentDegree(0)
{
    pid.setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
    pid.setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
    pid.setBias(BIAS);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(0.0);

    rawDegree = r1370.getRate();
    beforeDegree = rawDegree;
    offSetDegree = rawDegree;
}


void PIDC::confirm()
{
    r1370.update();
    rawDegree = r1370.getRate();
    if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
    if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
    currentDegree = rawDegree - offSetDegree + turnOverNumber * 360.0;
    r1370.update();
    beforeDegree = r1370.getRate();
    pid.setProcessValue(currentDegree);
    calculationResult = pid.compute();
}

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

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

float PIDC::getRawDegree()
{
    return r1370.getRate();
}

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

void PIDC::resetOffset()
{
    beforeDegree = r1370.getRate();
    offSetDegree = r1370.getRate();
    turnOverNumber = 0;
}