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 00076 00077 /* 00078 * Change Log: 00079 * 00080 * $Log: WalkAccelerationRestrictor.cpp,v $ 00081 * Revision 1.2 2004/06/02 17:18:24 spranger 00082 * MotionRequest cleanup 00083 * 00084 * Revision 1.1.1.1 2004/05/22 17:36:01 cvsadm 00085 * created new repository GT2004_WM 00086 * 00087 * Revision 1.1 2003/10/07 10:13:21 cvsadm 00088 * Created GT2004 (M.J.) 00089 * 00090 * Revision 1.1 2003/09/26 11:40:40 juengel 00091 * - sorted tools 00092 * - clean-up in DataTypes 00093 * 00094 * Revision 1.1.1.1 2003/07/02 09:40:28 cvsadm 00095 * created new repository for the competitions in Padova from the 00096 * tamara CVS (Tuesday 2:00 pm) 00097 * 00098 * removed unused solutions 00099 * 00100 * Revision 1.1 2003/06/04 00:43:45 loetzsch 00101 * put the GT2003WalkAccelerationRestrictor into two extra files in /Src/Tools 00102 * 00103 */ 00104
1.3.6