Tools/Actorics/WalkAccelerationRestrictor.cpp

Go to the documentation of this file.

/**
* @file WalkAccelerationRestrictor.cpp
*
* Implementation of class WalkAccelerationRestrictor
*
* @author Martin Lötzsch
*/

#include "WalkAccelerationRestrictor.h"
#include "Platform/SystemCall.h"

WalkAccelerationRestrictor::WalkAccelerationRestrictor(MotionRequest& motionRequest)
: mr(motionRequest)
{
}

void WalkAccelerationRestrictor::saveLastWalkParameters()
{
  if (mr.motionType != MotionRequest::walk)
  {
    lastTranslationX = 0;
    lastTranslationY = 0;
    lastRotation = 0;
  }
  else
  {
    lastTranslationX = mr.walkRequest.walkParams.translation.x;
    lastTranslationY = mr.walkRequest.walkParams.translation.y;
    lastRotation = mr.walkRequest.walkParams.rotation;
  }
}

void WalkAccelerationRestrictor::restrictAccelerations(double maxTranslationXAcceleration,
                                                       double maxTranslationYAcceleration,
                                                       double maxRotationAcceleration)
{
  unsigned long currentTime = SystemCall::getCurrentSystemTime();
  if (timeOfLastExecution == 0 || timeOfLastExecution > currentTime + 250)
    timeOfLastExecution = currentTime - 50;
  unsigned long timeSinceLastExecution = currentTime - timeOfLastExecution;
  if (timeSinceLastExecution > 1000) timeSinceLastExecution = 1000;

  double maximumTranslationXChange = 
    maxTranslationXAcceleration * timeSinceLastExecution / 1000;

  double maximumTranslationYChange = 
    maxTranslationYAcceleration * timeSinceLastExecution / 1000;

  double vMaximumRotationChange =
    maxRotationAcceleration * timeSinceLastExecution / 1000;

  double maximumRotationChange;
  maximumRotationChange = fromDegrees(vMaximumRotationChange);

  if (mr.walkRequest.walkParams.translation.x - lastTranslationX > maximumTranslationXChange)
    mr.walkRequest.walkParams.translation.x = lastTranslationX + maximumTranslationXChange;

  if (mr.walkRequest.walkParams.translation.x - lastTranslationX < -1 * maximumTranslationXChange)
    mr.walkRequest.walkParams.translation.x = lastTranslationX - maximumTranslationXChange;

  if (mr.walkRequest.walkParams.translation.y - lastTranslationY > maximumTranslationYChange)
    mr.walkRequest.walkParams.translation.y = lastTranslationY + maximumTranslationYChange;

  if (mr.walkRequest.walkParams.translation.y - lastTranslationY < -1 * maximumTranslationYChange)
    mr.walkRequest.walkParams.translation.y = lastTranslationY - maximumTranslationYChange;

  if (mr.walkRequest.walkParams.rotation - lastRotation > maximumRotationChange)
    mr.walkRequest.walkParams.rotation = lastRotation + maximumRotationChange;

  if (mr.walkRequest.walkParams.rotation - lastRotation < -1 * maximumRotationChange)
    mr.walkRequest.walkParams.rotation = lastRotation - maximumRotationChange;

  timeOfLastExecution = SystemCall::getCurrentSystemTime();
}

---