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-05
Revision:
20:477c5d039e93
Parent:
19:41f7dd1a5ed1
Child:
22:682cc376da6f

File content as of revision 20:477c5d039e93:

#include "PID_controller.h"

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

PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), JY901(HMCsda, HMCscl)
{
    PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
    PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
    PID::setBias(BIAS);
    PID::setMode(AUTO_MODE);
    PID::setSetPoint(0.0);

    //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
    rawDegree = JY901::getZaxisAngle();
    beforeDegree = JY901::getZaxisAngle();
    offSetDegree = JY901::getZaxisAngle();
    initDegree = 0;
    turnOverNumber = 0;
//    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)
{
    PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
    PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
    PID::setBias(BIAS);
    PID::setMode(AUTO_MODE);
    PID::setSetPoint(0.0);

    //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
    rawDegree = JY901::getZaxisAngle();
    beforeDegree = JY901::getZaxisAngle();
    offSetDegree = JY901::getZaxisAngle();
    initDegree = 0;
    turnOverNumber = 0;
//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
}


void PIDC::confirm()
{
    rawDegree = getZaxisAngle();
    if(rawDegree - beforeDegree < -1800) turnOverNumber++;
    if(rawDegree - beforeDegree > 1800) turnOverNumber--;
    initDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
    beforeDegree = JY901::getZaxisAngle();
    PID::setProcessValue(initDegree / 10.0);
    co = PID::compute();
}

float PIDC::getCo()
{
    return co;
}