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-09-13
Revision:
23:37bb9afe9fdc
Parent:
22:682cc376da6f
Child:
26:7258d5ad0bff

File content as of revision 23:37bb9afe9fdc:

#include "PID_controller.h"

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

PIDC::PIDC() :
    PID(KC, TI, TD, INTERVAL),
    JY901(HMCsda, HMCscl),
    axisOffSetDegree(0),
    planeOffSetDegree(0),
    turnOverNumber(0),
    beforeDegree(0),
    rawDegree(0),
    calculationResult(0),
    axisCurrentDegree(0),
    planeCurrentDegree(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 = JY901::getZaxisAngle();
    beforeDegree = rawDegree;
    planeOffSetDegree = rawDegree;
    axisOffSetDegree = rawDegree;
//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
}

PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
    PID(kc, ti, td, interval),
    JY901(sda, scl),
    axisOffSetDegree(0),
    planeOffSetDegree(0),
    turnOverNumber(0),
    beforeDegree(0),
    rawDegree(0),
    calculationResult(0),
    axisCurrentDegree(0),
    planeCurrentDegree(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 = JY901::getZaxisAngle();
    beforeDegree = rawDegree;
    planeOffSetDegree = rawDegree;
    axisOffSetDegree = rawDegree;
//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
}


void PIDC::confirm()
{
    rawDegree = JY901::getZaxisAngle();
    if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
    if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
    axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 360;
    planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 360;
    beforeDegree = JY901::getZaxisAngle();
    PID::setProcessValue(axisCurrentDegree);
    calculationResult = PID::compute();
}

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

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

void PIDC::resetAxisOffset()
{
    beforeDegree = JY901::getZaxisAngle();
    axisOffSetDegree = JY901::getZaxisAngle();
    turnOverNumber = 0;
}

void PIDC::resetPlaneOffset()
{
    beforeDegree = JY901::getZaxisAngle();
    planeOffSetDegree = JY901::getZaxisAngle();
    turnOverNumber = 0;
}

void PIDC::renewAngle()
{
  angle = JY901::getZaxisAngle();
}

void PIDC::calibration(int mode)
{

}