Robot

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Control.cpp Source File

Control.cpp

00001 
00002 #include "Control.h"
00003 
00004 Control::Control(float _KP, float _KV, float _KI,float _KA,float _KM ,float _SamplingTime, float _Scale, float _Offset,float _MaxTorque){
00005     KP = _KP;
00006     KV = _KV;
00007     KI = _KI;
00008     KA = _KA;
00009     KM = _KM;
00010     Scale = _Scale;
00011     Offset = _Offset;
00012     MaxTorque = _MaxTorque;
00013     SamplingTime = _SamplingTime;
00014     PrevErr = 0;
00015 }   
00016 float Control::GetKP(){
00017     return KP;    
00018 }
00019 float Control::GetKV(){
00020     return KV;    
00021 }
00022 float Control::GetKI(){
00023     return KI;    
00024 }
00025 float Control::SaturationControl(float value){
00026     float u = (value*KA*KM)/MaxTorque;
00027     if (u>1){
00028         u=1;
00029     }
00030     if (u<0){
00031         u=0;
00032     }
00033     u = u* Scale + Offset;
00034     return u;   
00035 }  
00036 float Control::ComputeP(float qref, float qi){
00037     float u=(((qref - qi) * KP)*KA*KM)/MaxTorque;
00038     if (u>1){
00039         u=1;
00040     }
00041     if (u<0){
00042         u=0;
00043     }
00044     u = u* Scale + Offset;
00045     return u;   
00046 }     
00047 float Control::ComputePD_FirstVersion(float qref, float qi, float qdotref, float qdoti){
00048     float u=(((qref - qi) * KP + (qdotref - qdoti) * KV)*KA*KM)/MaxTorque;
00049     if (u>1){
00050         u=1;
00051     }
00052     if (u<0){
00053         u=0;
00054     }
00055     u = u* Scale + Offset;
00056     return u;
00057 }
00058 float Control::ComputePD_SecondVersion(float qref, float qi, float qdoti){
00059     float u=(((qref - qi) * KP  - qdoti * KV)*KA*KM)/MaxTorque;
00060     if (u>1){
00061         u=1;
00062     }
00063     if (u<0){
00064         u=0;
00065     }
00066     u = u* Scale + Offset;
00067     return u;
00068 }
00069 float Control::ComputePID(float qref, float qi, float qdotref, float qdoti){
00070     
00071     float IntegralErr = PrevErr + (qref - qi);
00072     PrevErr = IntegralErr;
00073 
00074     float u=(((qref-qi) * KP + (qdotref - qdoti) * KV + IntegralErr * KI)*KA*KM)/MaxTorque;
00075     
00076     if (u>1){
00077         u=1;
00078     }
00079     if (u<0){
00080         u=0;
00081     }
00082     u = u* Scale + Offset;
00083     return u;
00084 }
00085 float Control::ComputePDG(float qref, float qi, float qdotref, float qdoti, float Tau){
00086     float u = (((qref-qi) * KP + (qdotref - qdoti) * KV)*KA*KM + Tau)/MaxTorque;
00087     if (u>1){
00088         u=1;
00089     }
00090     if (u<0){
00091         u=0;
00092     }
00093     u = u* Scale + Offset;
00094     return u;
00095 }