Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/Actorics/WalkAccelerationRestrictor.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file WalkAccelerationRestrictor.cpp
    00003 *
    00004 * Implementation of class WalkAccelerationRestrictor
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "WalkAccelerationRestrictor.h"
    00010 #include "Platform/SystemCall.h"
    00011
    00012 WalkAccelerationRestrictor::WalkAccelerationRestrictor(MotionRequest& motionRequest)
    00013 : mr(motionRequest)
    00014 {
    00015 }
    00016
    00017 void WalkAccelerationRestrictor::saveLastWalkParameters()
    00018 {
    00019   if (mr.motionType != MotionRequest::walk)
    00020   {
    00021     lastTranslationX = 0;
    00022     lastTranslationY = 0;
    00023     lastRotation = 0;
    00024   }
    00025   else
    00026   {
    00027     lastTranslationX = mr.walkRequest.walkParams.translation.x;
    00028     lastTranslationY = mr.walkRequest.walkParams.translation.y;
    00029     lastRotation = mr.walkRequest.walkParams.rotation;
    00030   }
    00031 }
    00032
    00033 void WalkAccelerationRestrictor::restrictAccelerations(double maxTranslationXAcceleration,
    00034                                                        double maxTranslationYAcceleration,
    00035                                                        double maxRotationAcceleration)
    00036 {
    00037   unsigned long currentTime = SystemCall::getCurrentSystemTime();
    00038   if (timeOfLastExecution == 0 || timeOfLastExecution > currentTime + 250)
    00039     timeOfLastExecution = currentTime - 50;
    00040   unsigned long timeSinceLastExecution = currentTime - timeOfLastExecution;
    00041   if (timeSinceLastExecution > 1000) timeSinceLastExecution = 1000;
    00042
    00043   double maximumTranslationXChange =
    00044     maxTranslationXAcceleration * timeSinceLastExecution / 1000;
    00045
    00046   double maximumTranslationYChange =
    00047     maxTranslationYAcceleration * timeSinceLastExecution / 1000;
    00048
    00049   double vMaximumRotationChange =
    00050     maxRotationAcceleration * timeSinceLastExecution / 1000;
    00051
    00052   double maximumRotationChange;
    00053   maximumRotationChange = fromDegrees(vMaximumRotationChange);
    00054
    00055   if (mr.walkRequest.walkParams.translation.x - lastTranslationX > maximumTranslationXChange)
    00056     mr.walkRequest.walkParams.translation.x = lastTranslationX + maximumTranslationXChange;
    00057
    00058   if (mr.walkRequest.walkParams.translation.x - lastTranslationX < -1 * maximumTranslationXChange)
    00059     mr.walkRequest.walkParams.translation.x = lastTranslationX - maximumTranslationXChange;
    00060
    00061   if (mr.walkRequest.walkParams.translation.y - lastTranslationY > maximumTranslationYChange)
    00062     mr.walkRequest.walkParams.translation.y = lastTranslationY + maximumTranslationYChange;
    00063
    00064   if (mr.walkRequest.walkParams.translation.y - lastTranslationY < -1 * maximumTranslationYChange)
    00065     mr.walkRequest.walkParams.translation.y = lastTranslationY - maximumTranslationYChange;
    00066
    00067   if (mr.walkRequest.walkParams.rotation - lastRotation > maximumRotationChange)
    00068     mr.walkRequest.walkParams.rotation = lastRotation + maximumRotationChange;
    00069
    00070   if (mr.walkRequest.walkParams.rotation - lastRotation < -1 * maximumRotationChange)
    00071     mr.walkRequest.walkParams.rotation = lastRotation - maximumRotationChange;
    00072
    00073   timeOfLastExecution = SystemCall::getCurrentSystemTime();
    00074 }
    00075