JointLimitMonitor.cpp
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #include "youbot/JointLimitMonitor.hpp"
00052 namespace youbot {
00053
00054 JointLimitMonitor::JointLimitMonitor(const YouBotJointStorage& jointParameters, const quantity<angular_acceleration>& jointAcceleration) {
00055
00056 this->storage = jointParameters;
00057 this->acceleration = jointAcceleration.value();
00058 brakingDistance = 0;
00059 isbraking = false;
00060 velocityWhenReachedLimit = 0;
00061
00062
00063
00064
00065 }
00066
00067 JointLimitMonitor::~JointLimitMonitor() {
00068
00069
00070 }
00071
00072 JointLimitMonitor::JointLimitMonitor(const JointLimitMonitor & source) {
00073
00074 this->storage = source.storage;
00075
00076 }
00077
00078 JointLimitMonitor & JointLimitMonitor::operator=(const JointLimitMonitor & source) {
00079
00080 this->storage = source.storage;
00081 return *this;
00082
00083 }
00084
00085 void JointLimitMonitor::checkLimitsPositionControl(const quantity<plane_angle>& setpoint) {
00086
00087 if (storage.gearRatio == 0) {
00088 throw std::out_of_range("A Gear Ratio of zero is not allowed");
00089 }
00090
00091 if (storage.encoderTicksPerRound == 0) {
00092 throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
00093 }
00094 if(storage.areLimitsActive){
00095
00096 quantity<plane_angle> lowLimit = ((double) this->storage.lowerLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00097 quantity<plane_angle> upLimit = ((double) this->storage.upperLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00098
00099 if (storage.inverseMovementDirection) {
00100 upLimit = ((double) -(this->storage.lowerLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00101 lowLimit = ((double) -(this->storage.upperLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00102 }
00103
00104
00105 if (!((setpoint < upLimit) && (setpoint > lowLimit))) {
00106 std::stringstream errorMessageStream;
00107 errorMessageStream << "The setpoint angle for joint "<< this->storage.jointName <<" is out of range. The valid range is between " << lowLimit << " and " << upLimit << " and it is: " << setpoint;
00108 throw std::out_of_range(errorMessageStream.str());
00109 }
00110 }
00111
00112 }
00113
00114 void JointLimitMonitor::checkLimitsEncoderPosition(const signed int& setpoint) {
00115
00116 if(storage.areLimitsActive){
00117 if (!((setpoint < this->storage.upperLimit) && (setpoint > this->storage.lowerLimit))) {
00118 std::stringstream errorMessageStream;
00119 errorMessageStream << "The setpoint angle for joint "<< this->storage.jointName <<" is out of range. The valid range is between " << this->storage.lowerLimit << " and " << this->storage.upperLimit << " and it is: " << setpoint;
00120 throw std::out_of_range(errorMessageStream.str());
00121 }
00122 }
00123
00124 }
00125
00126 void JointLimitMonitor::checkLimitsProcessData(const SlaveMessageInput& messageInput, SlaveMessageOutput& messageOutput) {
00127
00128 switch (messageOutput.controllerMode) {
00129 case POSITION_CONTROL:
00130 break;
00131 case VELOCITY_CONTROL:
00132 if (isbraking == false) {
00133 calculateBrakingDistance(messageInput);
00134 }
00135
00136 if ((messageInput.actualPosition < bevorLowerLimit && !(messageOutput.value > 0)) || (messageInput.actualPosition > bevorUpperLimit && !(messageOutput.value < 0))) {
00137
00138 messageOutput.value = this->calculateBrakingVelocity(messageInput.actualPosition);
00139 isbraking = true;
00140 } else {
00141 isbraking = false;
00142 }
00143
00144 break;
00145 case CURRENT_MODE:
00146 break;
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 default:
00163
00164 break;
00165
00166 }
00167
00168
00169 }
00170
00171 double JointLimitMonitor::calculateDamping(const int actualPosition) {
00172
00173 if(actualPosition <= storage.lowerLimit){
00174 return 0.0;
00175 }
00176 if(actualPosition >= storage.upperLimit){
00177 return 0.0;
00178 }
00179 if(actualPosition < bevorLowerLimit){
00180 return abs(((double)(actualPosition - storage.lowerLimit))/(bevorLowerLimit - storage.lowerLimit));
00181 }
00182 if(actualPosition > bevorUpperLimit){
00183 return abs(((double)(storage.upperLimit - actualPosition))/(storage.upperLimit - bevorUpperLimit));
00184 }
00185 return 0.0;
00186
00187
00188 }
00189
00190 void JointLimitMonitor::calculateBrakingDistance(const SlaveMessageInput& messageInput) {
00191
00192 actualVelocityRPS = (((double) messageInput.actualVelocity / 60.0) * storage.gearRatio * 2.0 * M_PI);
00193
00194 brakingDistance = abs((((actualVelocityRPS * actualVelocityRPS) / (2.0 * acceleration)) * ((double) storage.encoderTicksPerRound / (2.0 * M_PI))) / storage.gearRatio);
00195
00196 bevorLowerLimit = storage.lowerLimit + brakingDistance;
00197 bevorUpperLimit = storage.upperLimit - brakingDistance;
00198 velocityWhenReachedLimit = messageInput.actualVelocity;
00199
00200 }
00201
00202 int JointLimitMonitor::calculateBrakingVelocity(const int actualPosition) {
00203
00204 if(actualPosition <= storage.lowerLimit){
00205 return 0;
00206 }
00207 if(actualPosition >= storage.upperLimit){
00208 return 0;
00209 }
00210 if(actualPosition < bevorLowerLimit){
00211 distanceToLimit = ((double) (actualPosition - storage.lowerLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI);
00212 newVelocity = -sqrt(2.0*acceleration* distanceToLimit);
00213 return round((newVelocity / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
00214 }
00215 if(actualPosition > bevorUpperLimit){
00216 distanceToLimit = ((double) (storage.upperLimit - actualPosition) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI);
00217 newVelocity = sqrt(2.0*acceleration* distanceToLimit);
00218 return round((newVelocity / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
00219 }
00220 return 0;
00221
00222
00223 }
00224
00225
00226 }