Akihiro Nakabayashi / Mbed 2 deprecated MainBoard2018_Auto_Master_A_awasetekudsai

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

Fork of MainBoard2018_Auto_Master_A_neww by Taiki Maruyama

Communication/PID/PID.cpp

Committer:
kishibekairohan
Date:
2018-10-01
Revision:
5:3ae504b88679

File content as of revision 5:3ae504b88679:

#include "PID.h"
#include <stdlib.h>

namespace PID_SPACE
{
    
    PID::PID(double deltaTime)
    {
        this->deltaTime = deltaTime;
        this->dataRangeLower = 0;
        this->dataRangeUpper = 100;
        kp = 0;
        ki = 0;
        kd = 0;
    }
    
    PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
    {
        this->deltaTime = deltaTime;
        this->dataRangeLower = dataRangeLower;
        this->dataRangeUpper = dataRangeUpper;
        kp = 0;
        ki = 0;
        kd = 0;
    }
    
    PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
    {
        this->deltaTime = deltaTime;
        this->dataRangeLower = dataRangeLower;
        this->dataRangeUpper = dataRangeUpper;
        kp = KP;
        ki = KI;
        kd = KD;
    }
    
    void PID::SetParam(double KP, double KI, double KD)
    {
        kp = KP;
        ki = KI;
        kd = KD;
    }
    
    double PID::SetPV(double sensorData, double targetData)
    {
        double p, i, d;
        
        diff[0] = diff[1];
        diff[1] = targetData - sensorData;
        integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
        
        p = kp * diff[1];
        i = ki * integral;
        d = kd * ((diff[1] - diff[0]) / deltaTime);
        mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
        return mv;
    }
    
    double PID::GetMV()
    {
        return mv;
    }
    
    double PID::limit(double data,double lower,double upper)
    {
        if(data < lower)        return lower;
        else if(data > upper)   return upper;
        else                    return data;
    }
    
}