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

# Modules/MotionControl/MotionStabilizer.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file MotionStabilizer.cpp
    00003 *
    00004 * Implementation of class MotionStabilizer
    00005 *
    00006 * @author Jan Hoffmann
    00007 */
    00008
    00009 #include "MotionStabilizer.h"
    00010
    00011 MotionStabilizer::MotionStabilizer() :
    00012 accelX (0, 0.05, 0, 0, -20, 20, 500000),
    00013 accelY (0, 0.05, 0, 0, -20, 20, 500000),
    00014 accelZ (-9.81, 0.05, 0, 0, -20, 20, 500000),
    00015 xFore (1, .9, 0, 0, .2, 10, 500000),
    00016 xHind (1, .9, 0, 0, .2, 10, 500000),
    00017 yLeft (1, .9, 0, 0, .2, 10, 500000),
    00018 yRight (1, .9, 0, 0, .2, 10, 500000),
    00019 stabilizerScale(5.0)
    00020 {
    00021 }
    00022
    00023 bool MotionStabilizer::stabilize(
    00024                                  MotionRequest::MotionID lastMotionType,
    00025                                  const MotionRequest& motionRequest,
    00026                                  JointData& jointData,
    00027                                  OdometryData& odometryData,
    00028                                  const SensorDataBuffer& sensorDataBuffer)
    00029 {
    00030   if(lastMotionType != MotionRequest::walk) return false;
    00031   if (!motionRequest.stabilize) return 0;
    00032
    00033   for (int frame = 0; frame < sensorDataBuffer.numOfFrames; frame++)
    00034   {
    00035     accelX.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationX]/1000000.0);
    00036     accelY.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationY]/1000000.0);
    00037     accelZ.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationZ]/1000000.0);
    00038   }
    00039
    00040
    00041   xFore.approximateVal(1-(accelX.getVal() - 2)/stabilizerScale);
    00042   xHind.approximateVal(1+(accelX.getVal() - 2)/stabilizerScale);
    00043
    00044   yLeft.approximateVal(1-(accelY.getVal())/stabilizerScale);
    00045   yRight.approximateVal(1+(accelY.getVal())/stabilizerScale);
    00046
    00047
    00048   /* fore legs */
    00049   jointData.data[JointData::legFR1]  = (long )((double )jointData.data[JointData::legFR1] * yRight.getVal() * xFore.getVal());
    00050   jointData.data[JointData::legFR2] *= 1;
    00051   jointData.data[JointData::legFR3]  = (long )((double )jointData.data[JointData::legFR3] * yRight.getVal() * xFore.getVal());
    00052
    00053   jointData.data[JointData::legFL1]  = (long )((double )jointData.data[JointData::legFL1] * yLeft.getVal() * xFore.getVal());
    00054   jointData.data[JointData::legFL2] *= 1;
    00055   jointData.data[JointData::legFL3]  = (long )((double )jointData.data[JointData::legFL3] * yLeft.getVal() * xFore.getVal());
    00056
    00057   /* hind legs */
    00058   jointData.data[JointData::legHR1]  = (long )((double )jointData.data[JointData::legHR1] * yRight.getVal() * xHind.getVal());
    00059   jointData.data[JointData::legHR2] *= 1;
    00060   jointData.data[JointData::legHR3]  = (long )((double )jointData.data[JointData::legHR3] * yRight.getVal() * xHind.getVal());
    00061
    00062   jointData.data[JointData::legHL1]  = (long )((double )jointData.data[JointData::legHL1] * yLeft.getVal() * xHind.getVal());
    00063   jointData.data[JointData::legHL2] *= 1;
    00064   jointData.data[JointData::legHL3]  = (long )((double )jointData.data[JointData::legHL3] * yLeft.getVal() * xHind.getVal());
    00065
    00066   return true;
    00067 }