00001 #include "robodyn_utilities/MultiLoopController.h"
00002 #include "nasa_common_logging/Logger.h"
00003 #include <math.h>
00004
00005 MultiLoopController::MultiLoopController()
00006 : busVoltageRatio(0.)
00007 , velocityLoopIntegrator(0.)
00008 , previousDesiredCurrent(0.)
00009 , previousDesiredVelocity(0.)
00010 , loopRateSet(false)
00011 , positionLoopSet(false)
00012 , torqueLoopSet(false)
00013 , velocityLoopSet(false)
00014 , currentLoopSet(false)
00015 , hardwareSet(false)
00016 , ready(false)
00017 {
00018 }
00019
00020 MultiLoopController::MultiLoopController(const MultiLoopController& mlc_in)
00021 : busVoltageRatio(0.)
00022 , velocityLoopIntegrator(0.)
00023 , previousDesiredCurrent(0.)
00024 , previousDesiredVelocity(0.)
00025 , loopRateSet(false)
00026 , positionLoopSet(false)
00027 , torqueLoopSet(false)
00028 , velocityLoopSet(false)
00029 , currentLoopSet(false)
00030 , hardwareSet(false)
00031 , ready(false)
00032 {
00033 setMultiLoopParameters(mlc_in.getMultiLoopParameters());
00034 }
00035
00036 double MultiLoopController::update(double desiredPosition, double actualPosition, double actualVelocity, double busVoltage, double& desiredVelocity)
00037 {
00038 desiredVelocity = 0.0;
00039 double tempVel = 0.0;
00040
00041 if (!ready)
00042 {
00043 std::stringstream err;
00044 err << "not all parameters have been set";
00045 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00046 throw std::runtime_error(err.str());
00047 }
00048
00051
00053 if (!coeffs.positionLoopBypass)
00054 {
00055 Robodyn::limit(coeffs.minPosition, coeffs.maxPosition, desiredPosition);
00056 tempVel = coeffs.positionLoopKp * (desiredPosition - actualPosition);
00057 }
00058 else
00059 {
00060 tempVel = desiredPosition;
00061 }
00062
00064 return velocityLoop(tempVel, actualVelocity, busVoltage, desiredVelocity);
00065 }
00066
00067 double MultiLoopController::update(double desiredPosition, double kTendon, double actualPosition, double actualTension, double actualVelocity, double busVoltage, double& desiredVelocity)
00068 {
00069 desiredVelocity = 0.0;
00070 double tempVel = 0.0;
00071
00072 if (!ready)
00073 {
00074 std::stringstream err;
00075 err << "not all parameters have been set";
00076 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00077 throw std::runtime_error(err.str());
00078 }
00079
00081 if (!coeffs.torqueLoopBypass)
00082 {
00083 Robodyn::limit(coeffs.minTension, coeffs.maxTension, actualTension);
00084 desiredPosition += kTendon * coeffs.torqueLoopKp * (actualTension - coeffs.tensionOffset);
00085 }
00086
00087
00088
00090 if (!coeffs.positionLoopBypass)
00091 {
00092 Robodyn::limit(coeffs.minPosition, coeffs.maxPosition, desiredPosition);
00093 tempVel = coeffs.positionLoopKp * (desiredPosition - actualPosition);
00094 }
00095 else
00096 {
00097 tempVel = desiredPosition;
00098 }
00099
00101 return velocityLoop(tempVel, actualVelocity, busVoltage, desiredVelocity);
00102 }
00103
00104 void MultiLoopController::setMultiLoopParameters(const MultiLoopParams& mlp_in)
00105 {
00106 setLoopRate(mlp_in.loopRate);
00107 setPositionLoopParameters(mlp_in.minPosition, mlp_in.maxPosition, mlp_in.positionLoopKp, mlp_in.positionLoopBypass);
00108 setTorqueLoopParameters(mlp_in.minTension, mlp_in.maxTension, mlp_in.torqueLoopKp, mlp_in.tensionOffset, mlp_in.torqueLoopBypass);
00109 setVelocityLoopParameters(mlp_in.minVelocity, mlp_in.maxVelocity, mlp_in.velocityLoopKp, mlp_in.velocityLoopKi, mlp_in.velocityLoopIntegratorWindupLimit, mlp_in.velocityLoopBypass);
00110 setCurrentLoopParameters(mlp_in.minCurrent, mlp_in.maxCurrent, mlp_in.maxDutyCycle, mlp_in.currentLoopBypass);
00111 setHardwareParameters(mlp_in.motorViscousDampingCompensation, mlp_in.motorInertia, mlp_in.motorTorqueConstant, mlp_in.motorPhaseResistance,
00112 mlp_in.motorBackEMFConstant, mlp_in.motorInductance, mlp_in.motionRatio, mlp_in.PWMFreq, mlp_in.bridgeDeadTime, mlp_in.bridgeSwitchTime);
00113 }
00114
00115 void MultiLoopController::setLoopRate(double loopRate)
00116 {
00117 if (loopRate <= 0.)
00118 {
00119 std::stringstream err;
00120 err << "loopRate must be greater than 0";
00121 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00122 throw std::runtime_error(err.str());
00123 }
00124
00125 coeffs.loopRate = loopRate;
00126 loopRateSet = true;
00127 ready = loopRateSet && positionLoopSet && torqueLoopSet && velocityLoopSet && currentLoopSet && hardwareSet;
00128 }
00129
00130 void MultiLoopController::setPositionLoopParameters(double minPosition, double maxPosition, double Kp, bool bypass)
00131 {
00132 if (minPosition > maxPosition)
00133 {
00134 std::stringstream err;
00135 err << "minPosition is greater than maxPosition";
00136 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00137 throw std::runtime_error(err.str());
00138 }
00139
00140 coeffs.minPosition = minPosition;
00141 coeffs.maxPosition = maxPosition;
00142 coeffs.positionLoopKp = Kp;
00143 coeffs.positionLoopBypass = bypass;
00144 positionLoopSet = true;
00145 ready = loopRateSet && positionLoopSet && torqueLoopSet && velocityLoopSet && currentLoopSet && hardwareSet;
00146 }
00147
00148 void MultiLoopController::setTorqueLoopParameters(double minTension, double maxTension, double Kp, double offset, bool bypass)
00149 {
00150 if (minTension > maxTension)
00151 {
00152 std::stringstream err;
00153 err << "minTension is greater than maxTension";
00154 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00155 throw std::runtime_error(err.str());
00156 }
00157
00158 coeffs.minTension = minTension;
00159 coeffs.maxTension = maxTension;
00160 coeffs.torqueLoopKp = Kp;
00161 coeffs.tensionOffset = offset;
00162 coeffs.torqueLoopBypass = bypass;
00163 torqueLoopSet = true;
00164 ready = loopRateSet && positionLoopSet && torqueLoopSet && velocityLoopSet && currentLoopSet && hardwareSet;
00165 }
00166
00167 void MultiLoopController::setVelocityLoopParameters(double minVelocity, double maxVelocity, double Kp, double Ki, double integratorWindupLimit, bool bypass)
00168 {
00169 if (minVelocity > maxVelocity)
00170 {
00171 std::stringstream err;
00172 err << "minVelocity is greater than maxVelocity";
00173 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00174 throw std::runtime_error(err.str());
00175 }
00176 else if (integratorWindupLimit < 0)
00177 {
00178 std::stringstream err;
00179 err << "integratorWindupLimit is less than zero";
00180 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00181 throw std::runtime_error(err.str());
00182 }
00183
00184 coeffs.minVelocity = minVelocity;
00185 coeffs.maxVelocity = maxVelocity;
00186 coeffs.velocityLoopKp = Kp;
00187 coeffs.velocityLoopKi = Ki;
00188 coeffs.velocityLoopIntegratorWindupLimit = integratorWindupLimit;
00189 coeffs.velocityLoopBypass = bypass;
00190 velocityLoopSet = true;
00191 ready = loopRateSet && positionLoopSet && torqueLoopSet && velocityLoopSet && currentLoopSet && hardwareSet;
00192 }
00193
00194 void MultiLoopController::setCurrentLoopParameters(double minCurrent, double maxCurrent, double maxDutyCycle, bool bypass)
00195 {
00196 if (minCurrent > maxCurrent)
00197 {
00198 std::stringstream err;
00199 err << "minCurrent is greater than maxCurrent";
00200 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00201 throw std::runtime_error(err.str());
00202 }
00203 else if (maxDutyCycle < 0)
00204 {
00205 std::stringstream err;
00206 err << "maxDutyCycle is less than zero";
00207 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00208 throw std::runtime_error(err.str());
00209 }
00210
00211 coeffs.minCurrent = minCurrent;
00212 coeffs.maxCurrent = maxCurrent;
00213 coeffs.maxDutyCycle = maxDutyCycle;
00214 coeffs.currentLoopBypass = bypass;
00215 currentLoopSet = true;
00216 ready = loopRateSet && positionLoopSet && torqueLoopSet && velocityLoopSet && currentLoopSet && hardwareSet;
00217 }
00218
00219 void MultiLoopController::setHardwareParameters(double motorViscousDampingCompensation, double motorInertia, double motorTorqueConstant, double motorPhaseResistance,
00220 double motorBackEMFConstant, double motorInductance, double motionRatio, double PWMFreq, double bridgeDeadTime, double bridgeSwitchTime)
00221 {
00222 if (motorTorqueConstant == 0.)
00223 {
00224 std::stringstream err;
00225 err << "motorTorqueConstant must not be equal zero";
00226 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00227 throw std::runtime_error(err.str());
00228 }
00229 else if (motionRatio == 0.)
00230 {
00231 std::stringstream err;
00232 err << "motionRatio must not be equal zero";
00233 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00234 throw std::runtime_error(err.str());
00235 }
00236 else if ((busVoltageRatio = 1. - 2. * PWMFreq * bridgeDeadTime) == 0.)
00237 {
00238 std::stringstream err;
00239 err << "(1. - (PWMFreq * 2.) * bridgeDeadTime) must not be equal zero";
00240 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00241 throw std::runtime_error(err.str());
00242 }
00243
00244 coeffs.motorViscousDampingCompensation = motorViscousDampingCompensation;
00245 coeffs.motorInertia = motorInertia;
00246 coeffs.motorTorqueConstant = motorTorqueConstant;
00247 coeffs.motorPhaseResistance = motorPhaseResistance;
00248 coeffs.motorBackEMFConstant = motorBackEMFConstant;
00249 coeffs.motorInductance = motorInductance;
00250 coeffs.motionRatio = motionRatio;
00251 coeffs.PWMFreq = PWMFreq;
00252 coeffs.bridgeDeadTime = bridgeDeadTime;
00253 coeffs.bridgeSwitchTime = bridgeSwitchTime;
00254 hardwareSet = true;
00255 ready = loopRateSet && positionLoopSet && torqueLoopSet && velocityLoopSet && currentLoopSet && hardwareSet;
00256 }
00257
00258
00259 double MultiLoopController::velocityLoop(double desiredVelocity, double actualVelocity, double busVoltage, double& limitedVelocity)
00260 {
00261 double velocityError, desiredCurrentFF, desiredCurrent, desiredVelocityDot;
00262 limitedVelocity = desiredVelocity;
00263
00264 if (!coeffs.velocityLoopBypass)
00265 {
00267 Robodyn::limit(coeffs.minVelocity, coeffs.maxVelocity, limitedVelocity);
00268 desiredVelocityDot = (limitedVelocity - previousDesiredVelocity) * coeffs.loopRate;
00269 previousDesiredVelocity = limitedVelocity;
00270 velocityError = limitedVelocity - actualVelocity;
00271 velocityLoopIntegrator += coeffs.velocityLoopKi * velocityError;
00272 desiredCurrentFF = (limitedVelocity * coeffs.motorViscousDampingCompensation + coeffs.motorInertia * desiredVelocityDot) / coeffs.motorTorqueConstant;
00273 desiredCurrent = coeffs.velocityLoopKp * velocityError + desiredCurrentFF;
00274 double tempIntegratorWindup = std::max(coeffs.velocityLoopIntegratorWindupLimit - fabs(desiredCurrent), 0.);
00275 Robodyn::limit(-tempIntegratorWindup, tempIntegratorWindup, velocityLoopIntegrator);
00276 desiredCurrent += velocityLoopIntegrator;
00277 }
00278 else
00279 {
00280 desiredCurrent = limitedVelocity;
00281 }
00282
00284 return currentLoop(desiredCurrent, actualVelocity, busVoltage);
00285 }
00286
00287 double MultiLoopController::currentLoop(double desiredCurrent, double actualVelocity, double busVoltage)
00288 {
00289 double dutyCycleFF, desiredCurrentDot;
00290
00291 if (busVoltage == 0.)
00292 {
00293 std::stringstream err;
00294 err << "busVoltage must not be equal zero";
00295 NasaCommonLogging::Logger::log("gov.nasa.robonet.MultiLoopController", log4cpp::Priority::ERROR, err.str());
00296 throw std::runtime_error(err.str());
00297 }
00298
00299 if (!coeffs.currentLoopBypass)
00300 {
00302 Robodyn::limit(coeffs.minCurrent, coeffs.maxCurrent, desiredCurrent);
00303 desiredCurrentDot = (desiredCurrent - previousDesiredCurrent) * coeffs.loopRate;
00304 previousDesiredCurrent = desiredCurrent;
00305 dutyCycleFF = (desiredCurrent * coeffs.motorPhaseResistance + coeffs.motorBackEMFConstant * actualVelocity / coeffs.motionRatio
00306 + coeffs.motorInductance * desiredCurrentDot) * coeffs.maxDutyCycle / (busVoltageRatio * busVoltage);
00307
00308
00309
00310
00311 if (fabs(desiredCurrent) > 0.0001)
00312 {
00313 dutyCycleFF += (desiredCurrent < 0 ? -1 : 1.) * coeffs.bridgeSwitchTime * coeffs.PWMFreq * coeffs.maxDutyCycle;
00314 }
00315 }
00316 else
00317 {
00318 dutyCycleFF = desiredCurrent;
00319 }
00320
00322 Robodyn::limit(-coeffs.maxDutyCycle, coeffs.maxDutyCycle, dutyCycleFF);
00323
00324 return dutyCycleFF;
00325 }
00326
00327
00328