NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated NHK2017_octopus

NHK2017 octopus robot

Dependencies:   2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel

Fork of KANI2017v2 by NagaokaRoboticsClub_mbedTeam

bot/PIDcontroller/PID_controller.cpp

Committer:
number_key
Date:
2017-10-25
Revision:
26:7258d5ad0bff
Parent:
23:37bb9afe9fdc

File content as of revision 26:7258d5ad0bff:

#include "PID_controller.h"

void PIDC::updateOutput()
{
    confirm();
}

PIDC::PIDC() :
    PID(KC, TI, TD, INTERVAL),
    HMC6352(HMCsda, HMCscl),
    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);
    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(0.1);
    rawDegree = HMC6352::sample();
    beforeDegree = rawDegree;
    offSetDegree = rawDegree;
}

PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
    PID(kc, ti, td, interval),
    HMC6352(sda, scl),
    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);
    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(0.1);
    rawDegree = HMC6352::sample();
    beforeDegree = rawDegree;
    offSetDegree = rawDegree;
}


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

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

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

int PIDC::getRawDegree()
{
    return HMC6352::sample();
}

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

void PIDC::resetOffset()
{
    beforeDegree = HMC6352::sample();
    offSetDegree = HMC6352::sample();
    turnOverNumber = 0;
}

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