ROBOFRIEN_PID.cpp

00001 #include "ROBOFRIEN_PID.h"
00002 
00003 
00004 #define OUTPUT_LIMIT      3000
00005 #define RAT_INTEGER_LIMIT   500
00006 void ROBOFRIEN_PID::Init(){
00007     RAT_INTEGER[0] = 0;
00008     RAT_INTEGER[1] = 0;
00009     RAT_INTEGER[2] = 0;
00010 }
00011 
00012 void ROBOFRIEN_PID::update(){
00013     output_Data[0] = err_Rate[0] * RAT_KP[0];       // P GAIN    
00014     RAT_INTEGER[0] += err_Rate[0]/100.0 * RAT_KI[0];
00015     output_Data[0] += RAT_INTEGER[0];
00016     if(output_Data[0] > OUTPUT_LIMIT) output_Data[0] = OUTPUT_LIMIT;
00017     else if(output_Data[0] < -OUTPUT_LIMIT) output_Data[0] = -OUTPUT_LIMIT;
00018 
00019     output_Data[1] = err_Rate[1] * RAT_KP[1];       // P GAIN    
00020     RAT_INTEGER[1] += err_Rate[1]/100.0 * RAT_KI[1];
00021     output_Data[1] += RAT_INTEGER[1];
00022     if(output_Data[1] > OUTPUT_LIMIT) output_Data[1] = OUTPUT_LIMIT;
00023     else if(output_Data[1] < -OUTPUT_LIMIT) output_Data[1] = -OUTPUT_LIMIT;
00024 
00025     output_Data[2] = err_Rate[2] * RAT_KP[2];       // P GAIN    
00026     RAT_INTEGER[2] += err_Rate[2]/100.0 * RAT_KI[2];
00027     output_Data[2] += RAT_INTEGER[2];
00028     if(output_Data[2] > OUTPUT_LIMIT) output_Data[2] = OUTPUT_LIMIT;
00029     else if(output_Data[2] < -OUTPUT_LIMIT) output_Data[2] = -OUTPUT_LIMIT;
00030 }
00031 
00032 void ROBOFRIEN_PID::RAT_INTEGER_RESET(){
00033     RAT_INTEGER[0] = 0;
00034     RAT_INTEGER[1] = 0;
00035     RAT_INTEGER[2] = 0;
00036 }
00037