00001 00007 #ifndef MULTILOOPCONTROLLER_H 00008 #define MULTILOOPCONTROLLER_H 00009 00010 #include "robodyn_utilities/GeneralUtilities.h" 00011 00012 class MultiLoopController 00013 { 00014 public: 00015 MultiLoopController(); 00016 MultiLoopController(const MultiLoopController& mlc_in); 00017 virtual ~MultiLoopController() {} 00018 00019 struct MultiLoopParams 00020 { 00021 MultiLoopParams() 00022 : loopRate(0.) 00023 , positionLoopBypass(false) 00024 , minPosition(0.) 00025 , maxPosition(0.) 00026 , positionLoopKp(0.) 00027 , torqueLoopBypass(false) 00028 , minTension(0.) 00029 , maxTension(0.) 00030 , torqueLoopKp(0.) 00031 , tensionOffset(0.) 00032 , velocityLoopBypass(false) 00033 , minVelocity(0.) 00034 , maxVelocity(0.) 00035 , velocityLoopKp(0.) 00036 , velocityLoopKi(0.) 00037 , velocityLoopIntegratorWindupLimit(0.) 00038 , currentLoopBypass(false) 00039 , minCurrent(0.) 00040 , maxCurrent(0.) 00041 , maxDutyCycle(0.) 00042 , motorViscousDampingCompensation(0.) 00043 , motorInertia(0.) 00044 , motorTorqueConstant(0.) 00045 , motorPhaseResistance(0.) 00046 , motorBackEMFConstant(0.) 00047 , motorInductance(0.) 00048 , motionRatio(0.) 00049 , PWMFreq(0.) 00050 , bridgeDeadTime(0.) 00051 , bridgeSwitchTime(0.) 00052 {} 00053 00054 double loopRate; 00056 bool positionLoopBypass; 00057 double minPosition; 00058 double maxPosition; 00059 double positionLoopKp; 00061 bool torqueLoopBypass; 00062 double minTension; 00063 double maxTension; 00064 double torqueLoopKp; 00065 double tensionOffset; 00067 bool velocityLoopBypass; 00068 double minVelocity; 00069 double maxVelocity; 00070 double velocityLoopKp; 00071 double velocityLoopKi; 00072 double velocityLoopIntegratorWindupLimit; 00074 bool currentLoopBypass; 00075 double minCurrent; 00076 double maxCurrent; 00077 double maxDutyCycle; 00079 double motorViscousDampingCompensation; 00080 double motorInertia; 00081 double motorTorqueConstant; 00082 double motorPhaseResistance; 00083 double motorBackEMFConstant; 00084 double motorInductance; 00085 double motionRatio; 00086 double PWMFreq; 00087 double bridgeDeadTime; 00088 double bridgeSwitchTime; 00089 }; 00090 00100 double update(double desiredPosition, double actualPosition, double actualVelocity, double busVoltage, double& desiredVelocity); 00112 double update(double desiredPosition, double kTendon, double actualPosition, double actualTension, double actualVelocity, double busVoltage, double& desiredVelocity); 00113 00114 inline void reset() {velocityLoopIntegrator = 0.; previousDesiredCurrent = 0.; previousDesiredVelocity = 0.;} 00115 00116 void setMultiLoopParameters(const MultiLoopParams& mlp_in); 00117 const MultiLoopParams& getMultiLoopParameters() const {return coeffs;} 00118 00123 void setLoopRate(double loopRate); 00128 void setPositionLoopParameters(double minPosition, double maxPosition, double Kp, bool bypass = false); 00133 void setTorqueLoopParameters(double minTension, double maxTension, double Kp, double offset, bool bypass = false); 00139 void setVelocityLoopParameters(double minVelocity, double maxVelocity, double Kp, double Ki, double integratorWindupLimit, bool bypass = false); 00145 void setCurrentLoopParameters(double minCurrent, double maxCurrent, double maxDutyCycle, bool bypass = false); 00152 void setHardwareParameters(double motorViscousDampingCompensation, double motorInertia, double motorTorqueConstant, double motorPhaseResistance, 00153 double motorBackEMFConstant, double motorInductance, double motionRatio, double PWMFreq, double bridgeDeadTime, 00154 double bridgeSwitchTime); 00155 00157 00160 double velocityLoop(double desiredVelocity, double actualVelocity, double busVoltage, double& limitedVelocity); 00165 double currentLoop(double desiredCurrent, double actualVelocity, double busVoltage); 00166 00167 private: 00168 MultiLoopParams coeffs; 00169 double busVoltageRatio; 00170 double velocityLoopIntegrator; 00171 double previousDesiredCurrent; 00172 double previousDesiredVelocity; 00173 bool loopRateSet; 00174 bool positionLoopSet; 00175 bool torqueLoopSet; 00176 bool velocityLoopSet; 00177 bool currentLoopSet; 00178 bool hardwareSet; 00179 bool ready; 00180 }; 00181 00182 #endif