Kim Youngsik / Mbed 2 deprecated 0ROBOFRIEN_FCC_v1_12

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:   mbed BufferedSerial ConfigFile

ROBOFRIEN_SUB_FUNC/ROBOFRIEN_PID.cpp

Committer:
skyyoungsik
Date:
2018-11-28
Revision:
1:9530746906b6
Parent:
ROBOFRIEN_SUB_FUNC/ROBOFRIE_PID.cpp@ 0:3473b92e991e

File content as of revision 1:9530746906b6:

#include "ROBOFRIEN_PID.h"


#define OUTPUT_LIMIT      3000
#define RAT_INTEGER_LIMIT   500
void ROBOFRIEN_PID::Init(){
    RAT_INTEGER[0] = 0;
    RAT_INTEGER[1] = 0;
    RAT_INTEGER[2] = 0;
}

void ROBOFRIEN_PID::update(){
    output_Data[0] = err_Rate[0] * RAT_KP[0];       // P GAIN    
    RAT_INTEGER[0] += err_Rate[0]/100.0 * RAT_KI[0];
    output_Data[0] += RAT_INTEGER[0];
    if(output_Data[0] > OUTPUT_LIMIT) output_Data[0] = OUTPUT_LIMIT;
    else if(output_Data[0] < -OUTPUT_LIMIT) output_Data[0] = -OUTPUT_LIMIT;

    output_Data[1] = err_Rate[1] * RAT_KP[1];       // P GAIN    
    RAT_INTEGER[1] += err_Rate[1]/100.0 * RAT_KI[1];
    output_Data[1] += RAT_INTEGER[1];
    if(output_Data[1] > OUTPUT_LIMIT) output_Data[1] = OUTPUT_LIMIT;
    else if(output_Data[1] < -OUTPUT_LIMIT) output_Data[1] = -OUTPUT_LIMIT;

    output_Data[2] = err_Rate[2] * RAT_KP[2];       // P GAIN    
    RAT_INTEGER[2] += err_Rate[2]/100.0 * RAT_KI[2];
    output_Data[2] += RAT_INTEGER[2];
    if(output_Data[2] > OUTPUT_LIMIT) output_Data[2] = OUTPUT_LIMIT;
    else if(output_Data[2] < -OUTPUT_LIMIT) output_Data[2] = -OUTPUT_LIMIT;
}

void ROBOFRIEN_PID::RAT_INTEGER_RESET(){
    RAT_INTEGER[0] = 0;
    RAT_INTEGER[1] = 0;
    RAT_INTEGER[2] = 0;
}