JointLimitMonitor.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public 
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/
#include <youbot_driver/youbot/JointLimitMonitor.hpp>
namespace youbot
{

JointLimitMonitor::JointLimitMonitor(const YouBotJointStorage& jointParameters,
                                     const quantity<angular_acceleration>& jointAcceleration)
{
  // Bouml preserved body begin 000FAB71
  this->storage = jointParameters;
  this->acceleration = jointAcceleration.value();  // rad/s^2
  brakingDistance = 0;
  isbraking = false;
  velocityWhenReachedLimit = 0;

  // Bouml preserved body end 000FAB71
}

JointLimitMonitor::~JointLimitMonitor()
{
  // Bouml preserved body begin 000FABF1
  // Bouml preserved body end 000FABF1
}

JointLimitMonitor::JointLimitMonitor(const JointLimitMonitor & source)
{
  // Bouml preserved body begin 000FAC71
  this->storage = source.storage;
  // Bouml preserved body end 000FAC71
}

JointLimitMonitor & JointLimitMonitor::operator=(const JointLimitMonitor & source)
{
  // Bouml preserved body begin 000FACF1
  this->storage = source.storage;
  return *this;
  // Bouml preserved body end 000FACF1
}

void JointLimitMonitor::checkLimitsPositionControl(const quantity<plane_angle>& setpoint)
{
  // Bouml preserved body begin 000FAD71
  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of zero is not allowed");
  }

  if (storage.encoderTicksPerRound == 0)
  {
    throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
  }
  if (storage.areLimitsActive)
  {

    quantity<plane_angle> lowLimit = ((double)this->storage.lowerLimit / storage.encoderTicksPerRound)
        * storage.gearRatio * (2.0 * M_PI) * radian;
    quantity<plane_angle> upLimit = ((double)this->storage.upperLimit / storage.encoderTicksPerRound)
        * storage.gearRatio * (2.0 * M_PI) * radian;

    if (storage.inverseMovementDirection)
    {
      upLimit = ((double)-(this->storage.lowerLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI)
          * radian;
      lowLimit = ((double)-(this->storage.upperLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI)
          * radian;
    }

    if (!((setpoint < upLimit) && (setpoint > lowLimit)))
    {
      std::stringstream errorMessageStream;
      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;
      throw std::out_of_range(errorMessageStream.str());
    }
  }
  // Bouml preserved body end 000FAD71
}

void JointLimitMonitor::checkLimitsEncoderPosition(const signed int& setpoint)
{
  // Bouml preserved body begin 000FAF71
  if (storage.areLimitsActive)
  {
    if (!((setpoint < this->storage.upperLimit) && (setpoint > this->storage.lowerLimit)))
    {
      std::stringstream errorMessageStream;
      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;
      throw std::out_of_range(errorMessageStream.str());
    }
  }
  // Bouml preserved body end 000FAF71
}

void JointLimitMonitor::checkLimitsProcessData(const SlaveMessageInput& messageInput, SlaveMessageOutput& messageOutput)
{
  // Bouml preserved body begin 000FCAF1
  switch (messageOutput.controllerMode)
  {
    case POSITION_CONTROL:
      break;
    case VELOCITY_CONTROL:
      if (isbraking == false)
      {
        calculateBrakingDistance(messageInput);
      }

      if ((messageInput.actualPosition < bevorLowerLimit && !(messageOutput.value > 0))
          || (messageInput.actualPosition > bevorUpperLimit && !(messageOutput.value < 0)))
      {
        //      messageOutput.value = velocityWhenReachedLimit * this->calculateDamping(messageInput.actualPosition);
        messageOutput.value = this->calculateBrakingVelocity(messageInput.actualPosition);
        isbraking = true;
      }
      else
      {
        isbraking = false;
      }

      break;
    case CURRENT_MODE:
      break; //disable limit checker for current mode
      /*
       if(isbraking == false){
       calculateBrakingDistance(messageInput);
       }

       if( (messageInput.actualPosition < bevorLowerLimit && !(messageOutput.value > 0)) || (messageInput.actualPosition > bevorUpperLimit && !(messageOutput.value < 0))) {
       messageOutput.value = this->calculateBrakingVelocity(messageInput.actualPosition);
       messageOutput.controllerMode = VELOCITY_CONTROL;
       isbraking = true;
       }else{
       isbraking = false;
       }

       break;
       */
    default:

      break;

  }

  // Bouml preserved body end 000FCAF1
}

double JointLimitMonitor::calculateDamping(const int actualPosition)
{
  // Bouml preserved body begin 000FAFF1
  if (actualPosition <= storage.lowerLimit)
  {
    return 0.0;
  }
  if (actualPosition >= storage.upperLimit)
  {
    return 0.0;
  }
  if (actualPosition < bevorLowerLimit)
  {
    return abs(((double)(actualPosition - storage.lowerLimit)) / (bevorLowerLimit - storage.lowerLimit));
  }
  if (actualPosition > bevorUpperLimit)
  {
    return abs(((double)(storage.upperLimit - actualPosition)) / (storage.upperLimit - bevorUpperLimit));
  }
  return 0.0;

  // Bouml preserved body end 000FAFF1
}

void JointLimitMonitor::calculateBrakingDistance(const SlaveMessageInput& messageInput)
{
  // Bouml preserved body begin 000FE471
  actualVelocityRPS = (((double)messageInput.actualVelocity / 60.0) * storage.gearRatio * 2.0 * M_PI); // radian_per_second;

  brakingDistance = abs(
      (((actualVelocityRPS * actualVelocityRPS) / (2.0 * acceleration))
          * ((double)storage.encoderTicksPerRound / (2.0 * M_PI))) / storage.gearRatio);

  bevorLowerLimit = storage.lowerLimit + brakingDistance;
  bevorUpperLimit = storage.upperLimit - brakingDistance;
  velocityWhenReachedLimit = messageInput.actualVelocity;
  // Bouml preserved body end 000FE471
}

int JointLimitMonitor::calculateBrakingVelocity(const int actualPosition)
{
  // Bouml preserved body begin 000FE4F1
  if (actualPosition <= storage.lowerLimit)
  {
    return 0;
  }
  if (actualPosition >= storage.upperLimit)
  {
    return 0;
  }
  if (actualPosition < bevorLowerLimit)
  {
    distanceToLimit = ((double)(actualPosition - storage.lowerLimit) / storage.encoderTicksPerRound) * storage.gearRatio
        * (2.0 * M_PI);
    newVelocity = -sqrt(2.0 * acceleration * distanceToLimit);
    return round((newVelocity / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
  }
  if (actualPosition > bevorUpperLimit)
  {
    distanceToLimit = ((double)(storage.upperLimit - actualPosition) / storage.encoderTicksPerRound) * storage.gearRatio
        * (2.0 * M_PI);
    newVelocity = sqrt(2.0 * acceleration * distanceToLimit);
    return round((newVelocity / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
  }
  return 0;

  // Bouml preserved body end 000FE4F1
}

} // namespace youbot