NagaokaRoboticsClub_mbedTeam / Mbed OS KANIv3

kani

Dependencies:   2017NHKpin_config FEP omni_wheel PID R1307 ikarashiMDC

classDiagram

　   ＼￣＼　　　　　　　　　　　　　    　 ／￣／ 
/l　    ＼　　＼　　 　　　　　　　　　 ／　　／　lヽ　 
|　ヽ 　ヽ　　 |　　　　 　　　　　　｜　　/　　/　| 
＼　` ‐ヽ　　ヽ　　●　　      ●    　　   /　　/　‐　 ／ 
　 ＼ __　l　　|　　｜|＿＿＿|｜ /　　l　__ ／ 
　　　 　＼　 ＼ ／　　　　　　＼／ 
　　　　　 ／＼|　　 人＿＿人　　|／＼　　　　<ｽﾞﾜｲｶﾞﾆ　 
　　　　／／＼|　　　　　      　　|／＼＼　　　　 
　　　　／／＼|　　　ｹｶﾞﾆ           　|／＼＼　　　　 
　　　　／　  　 . ＼＿＿＿＿＿／　 　      ＼ 

　 　 　 　 　 　 　 　 　 　 　 　    　 　 ┏┓　 　 　 　 ┏━┓┏┓　 　 　 　 　 　 　 
     ┏┓　      　 ┏┓┏┓   ┏┓　 　 ┏┓┗┛   　 ┏┓　┗┓┃┗┛　 　 　 　 　 　 　 
┏┛┗━┓ 　┃┃┃┃    ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ 
┗┓┏━┛　 ┃┃┗┛    ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛　 　 
┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ 　    ┏┛┃┃┃  　 　 　 ┃┃　 　 　 　 　 　 　 
┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ 　    ┗━┛┃┃  　 　 　 ┃┃┏┓　 　 　 　 　 
┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃　 　  　 　 ┏┛┃  　 　 　 ┃┃┃┗━━┓　 　 
┗┛┗━━┛┗━━━┛┗┛┗━━┛　 　  　 　 ┗━┛　   　 　 ┗┛┗━━━┛　

bot/PIDcontroller/PID_controller.cpp

Committer:
uchitake
Date:
2017-09-26
Branch:
develop1
Revision:
17:79fa65706f92
Parent:
6:fe9767a50891
Child:
23:797d25f3df5e
Child:
32:b619c7787dc3

File content as of revision 17:79fa65706f92:

#include "PID_controller.h"

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

PIDC::PIDC() :
    PID(KC, TI, TD, INTERVAL),
    HMC6352(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);

    wait(0.1);
    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(0.1);
    rawDegree = HMC6352::sample();
    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),
    HMC6352(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);

    wait(0.1);
    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(0.1);
    rawDegree = HMC6352::sample();
    beforeDegree = rawDegree;
    planeOffSetDegree = rawDegree;
    axisOffSetDegree = rawDegree;
//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
}


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

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

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

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

void PIDC::resetAxisOffset()
{
    beforeDegree = HMC6352::sample();
    axisOffSetDegree = HMC6352::sample();
    turnOverNumber = 0;
}

void PIDC::resetPlaneOffset()
{
    beforeDegree = HMC6352::sample();
    planeOffSetDegree = HMC6352::sample();
    turnOverNumber = 0;
}

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