Robot
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sun Jul 31 2022 05:13:07 by
1.7.2