Representations/Motion/PIDData.cpp

Go to the documentation of this file.

/**
* @file PIDData.cpp
*
* Implementation of class PIDData
* 
* @author Max Risler
*/

#include "PIDData.h"
#include "Platform/SystemCall.h"
#include "Tools/RobotConfiguration.h"

PIDData::PIDData()
{
  setToDefaults();
}

PIDData& PIDData::operator=(const PIDData& other)
{
  for (int j = 0; j < JointData::numOfJoint; j++)
  {
    p[j] = other.p[j];
    i[j] = other.i[j];
    d[j] = other.d[j];
  }
  return *this;
}

PIDData::PIDData(const PIDData& other)
{
  *this = other;
}

void PIDData::setToDefaults()
{
  bool isERS210 = getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210;
  static const int pERS210[] = {0x0A,0x0D,0x0A,0x0E,0x00,0x00,0x16,0x14,0x23,0x16,0x14,0x23,0x16,0x14,0x23,0x16,0x14,0x23,0x0A,0x07},
                   iERS210[] = {0x08,0x08,0x08,0x08,0x00,0x00,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x00,0x00},
                   dERS210[] = {0x0C,0x0B,0x0C,0x10,0x00,0x00,0x08,0x06,0x05,0x08,0x06,0x05,0x08,0x06,0x05,0x08,0x06,0x05,0x18,0x11},
                   pERS7[] =   {0x0A,0x08,0x08,0x08,0x00,0x00,0x1C,0x14,0x1C,0x1C,0x14,0x1C,0x1C,0x14,0x1C,0x1C,0x14,0x1C,0x0A,0x0A},
                   iERS7[] =   {0x04,0x02,0x04,0x00,0x00,0x00,0x08,0x04,0x08,0x08,0x04,0x08,0x08,0x04,0x08,0x08,0x04,0x08,0x04,0x04},
                   dERS7[] =   {0x02,0x04,0x02,0x04,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x04,0x04};
  const int* pp = isERS210 ? pERS210 : pERS7,
           * ii = isERS210 ? iERS210 : iERS7,
           * dd = isERS210 ? dERS210 : dERS7;

  for(int j = 0; j < JointData::numOfJoint; ++j)
  {
    p[j] = pp[j];
    i[j] = ii[j];
    d[j] = dd[j];
  }
}

void PIDData::setValues(JointData::JointID joint, int ap, int ai, int ad)
{
  p[joint] = ap;
  i[joint] = ai;
  d[joint] = ad;
}

void PIDData::setLegFJ1Values(int ap, int ai, int ad)
{
  setValues(JointData::legFR1, ap, ai, ad);
  setValues(JointData::legFL1, ap, ai, ad);
}

void PIDData::setLegHJ1Values(int ap, int ai, int ad)
{
  setValues(JointData::legHR1, ap, ai, ad);
  setValues(JointData::legHL1, ap, ai, ad);
}

void PIDData::setLegFJ2Values(int ap, int ai, int ad)
{
  setValues(JointData::legFR2, ap, ai, ad);
  setValues(JointData::legFL2, ap, ai, ad);
}

void PIDData::setLegHJ2Values(int ap, int ai, int ad)
{
  setValues(JointData::legHR2, ap, ai, ad);
  setValues(JointData::legHL2, ap, ai, ad);
}

void PIDData::setLegFJ3Values(int ap, int ai, int ad)
{
  setValues(JointData::legFR3, ap, ai, ad);
  setValues(JointData::legFL3, ap, ai, ad);
}

void PIDData::setLegHJ3Values(int ap, int ai, int ad)
{
  setValues(JointData::legHR3, ap, ai, ad);
  setValues(JointData::legHL3, ap, ai, ad);
}

In& operator>>(In& stream,PIDData& pidData)
{
  for (int i = 0; i < JointData::numOfJoint; i++)
  {
    stream >> pidData.p[i];
    stream >> pidData.i[i];
    stream >> pidData.d[i];
  }
  return stream;
}
 
Out& operator<<(Out& stream, const PIDData& pidData)
{
  for (int i = 0; i < JointData::numOfJoint; i++)
  {
    stream << pidData.p[i];
    stream << pidData.i[i];
    stream << pidData.d[i];
  }
  return stream;
}

/*
 * Change log :
 * 
 * $Log: PIDData.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:25:30  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.3  2003/12/31 15:29:29  roefer
 * Joints and LEDs for ERS-7
 *
 * Revision 1.2  2003/10/29 13:20:13  juengel
 * added method "setToDefaults"
 *
 * Revision 1.1  2003/10/07 10:07:01  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.5  2003/05/16 17:02:17  roefer
 * JAM removed
 *
 * Revision 1.4  2002/11/28 18:12:39  dueffert
 * default PID values changed (according to Sony recommendation)
 *
 * Revision 1.3  2002/11/26 17:17:37  risler
 * some comments added
 *
 * Revision 1.2  2002/11/25 15:16:34  dueffert
 * old logs removed
 *
 * Revision 1.1  2002/11/19 17:13:05  risler
 * added datatype PIDData
 * support for sending new pid values at runtime
 *
 */

---