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:
UCHITAKE
Date:
2017-08-22
Revision:
2:ea151e05033a

File content as of revision 2:ea151e05033a:

#include "PID_controller.h"

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

PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), HMC6352(Sensor3pin5a, Sensor3pin5a)
{
    setInputLimits(-M_PI, M_PI);
    setOutputLimits(-1.0, 1.0);
    setBias(0.0);
    setMode(AUTO_MODE);
    setSetPoint(0.0);

    setOpMode(HMC6352_CONTINUOUS, 1, 20);
    rawDegree = sample();
    beforeDegree = sample();
    offSetDegree = sample();
    initDegree = 0;
    turnOverNumber = 0;
    
    this -> attach(this, &PIDC::updateOutput, 0.05);
}