Prueba

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Controller.cpp Source File

Controller.cpp

00001 #include "Controller.h"
00002 
00003 Controller::Controller(float _KP, float _KV, float _KI, float _SamplingTime){
00004     KP = _KP;
00005     KV = _KV;
00006     KI = _KI;
00007     SamplingTime = _SamplingTime;
00008     PrevErr = 0;
00009 }    
00010 float Controller::ComputeP(float qref, float qi){
00011  return (qref - qi) * KP;   
00012 }     
00013 float Controller::ComputePD(float qref, float qi, float qdotref, float qdoti){
00014     return (qref - qi) * KP + (qdotref - qdoti) * KV;
00015 }
00016 /*float Controller::ComputePD(float qref, float qi, float qdotref, float qdoti){
00017     return (qref - qi) * KP  - qdoti * KV;
00018 }*/
00019 float Controller::ComputePID(float qref, float qi, float qdotref, float qdoti){
00020     
00021     float IntegralErr = PrevErr + (qref - qi);
00022     PrevErr = IntegralErr;
00023     return (qref-qi) * KP + (qdotref - qdoti) * KV + IntegralErr * KI;
00024  }