Modules/MotionControl/MotionStabilizer.cpp

Go to the documentation of this file.

/**
* @file MotionStabilizer.cpp
* 
* Implementation of class MotionStabilizer
*
* @author Jan Hoffmann
*/

#include "MotionStabilizer.h"

MotionStabilizer::MotionStabilizer() : 
accelX (0, 0.05, 0, 0, -20, 20, 500000),
accelY (0, 0.05, 0, 0, -20, 20, 500000),
accelZ (-9.81, 0.05, 0, 0, -20, 20, 500000),
xFore (1, .9, 0, 0, .2, 10, 500000),
xHind (1, .9, 0, 0, .2, 10, 500000),
yLeft (1, .9, 0, 0, .2, 10, 500000),
yRight (1, .9, 0, 0, .2, 10, 500000),
stabilizerScale(5.0)
{
}

bool MotionStabilizer::stabilize(
                                 MotionRequest::MotionID lastMotionType,
                                 const MotionRequest& motionRequest, 
                                 JointData& jointData, 
                                 OdometryData& odometryData,
                                 const SensorDataBuffer& sensorDataBuffer)
{
  if(lastMotionType != MotionRequest::walk) return false;   
  if (!motionRequest.stabilize) return 0;

  for (int frame = 0; frame < sensorDataBuffer.numOfFrames; frame++) 
  {    
    accelX.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationX]/1000000.0); 
    accelY.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationY]/1000000.0); 
    accelZ.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationZ]/1000000.0); 
  }
 
    
  xFore.approximateVal(1-(accelX.getVal() - 2)/stabilizerScale);
  xHind.approximateVal(1+(accelX.getVal() - 2)/stabilizerScale);

  yLeft.approximateVal(1-(accelY.getVal())/stabilizerScale);
  yRight.approximateVal(1+(accelY.getVal())/stabilizerScale);


  /* fore legs */
  jointData.data[JointData::legFR1]  = (long )((double )jointData.data[JointData::legFR1] * yRight.getVal() * xFore.getVal());
  jointData.data[JointData::legFR2] *= 1;
  jointData.data[JointData::legFR3]  = (long )((double )jointData.data[JointData::legFR3] * yRight.getVal() * xFore.getVal());

  jointData.data[JointData::legFL1]  = (long )((double )jointData.data[JointData::legFL1] * yLeft.getVal() * xFore.getVal());
  jointData.data[JointData::legFL2] *= 1;
  jointData.data[JointData::legFL3]  = (long )((double )jointData.data[JointData::legFL3] * yLeft.getVal() * xFore.getVal());

  /* hind legs */
  jointData.data[JointData::legHR1]  = (long )((double )jointData.data[JointData::legHR1] * yRight.getVal() * xHind.getVal());
  jointData.data[JointData::legHR2] *= 1;
  jointData.data[JointData::legHR3]  = (long )((double )jointData.data[JointData::legHR3] * yRight.getVal() * xHind.getVal());

  jointData.data[JointData::legHL1]  = (long )((double )jointData.data[JointData::legHL1] * yLeft.getVal() * xHind.getVal());
  jointData.data[JointData::legHL2] *= 1;
  jointData.data[JointData::legHL3]  = (long )((double )jointData.data[JointData::legHL3] * yLeft.getVal() * xHind.getVal());

  return true;
}


/*
 * Change log :
 * 
 * $Log: MotionStabilizer.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:20:37  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.1  2003/10/06 14:10:13  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.10  2003/06/13 17:05:43  jhoffman
 * largely simplified code
 *
 * Revision 1.9  2003/05/23 13:08:26  goehring
 * Number of Sensorvalues changed from 4 to numOfValues (BETA)
 *
 * Revision 1.8  2003/05/23 11:30:12  goehring
 * MotionStabilizer signature changed
 *
 * Revision 1.7  2003/05/23 09:36:51  goehring
 * MotionStabilizer gets all 4 Sensor Values
 *
 * Revision 1.6  2003/02/17 10:57:33  dueffert
 * warnings removed
 *
 * Revision 1.5  2002/12/13 09:53:50  jhoffman
 * works a lot better now, cleaning up still needs to be done
 *
 * Revision 1.4  2002/11/19 17:38:31  dueffert
 * doxygen bugs corrected
 *
 * Revision 1.3  2002/11/19 15:43:03  dueffert
 * doxygen comments corrected
 *
 * Revision 1.2  2002/09/22 18:40:53  risler
 * added new math functions, removed GTMath library
 *
 * Revision 1.1  2002/09/10 15:36:15  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed challenge related source code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.1.1.1  2002/05/10 12:40:15  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.5  2002/04/25 14:50:36  kallnik
 * changed double/float to double
 * added several #include GTMath
 *
 * PLEASE use double
 *
 * Revision 1.4  2002/04/23 15:08:44  risler
 * changed MotionRequest: walk instead of normalWalk,... and walkType added
 *
 * Revision 1.3  2002/04/23 15:00:06  jhoffman
 * changes and additions
 *
 * Revision 1.2  2002/04/05 14:08:56  jhoffman
 * stabilizer stuff
 *
 * Revision 1.1  2002/04/04 15:24:09  jhoffman
 * added motion stabilizer to motion control
 *
 *
 *
 */

---