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::setHeadToDefaults(bool neckTilt, bool headPan, bool headTilt, bool mouth, bool earL, bool earR)
{
  const bool set[] = { neckTilt, headPan,headTilt,mouth,earL,earR};
  bool isERS210 = getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210;
  static const int pERS210[] = {0x0A,0x0D,0x0A,0x0E,0x00,0x00},
                   iERS210[] = {0x08,0x08,0x08,0x08,0x00,0x00},
                   dERS210[] = {0x0C,0x0B,0x0C,0x10,0x00,0x00},
                   pERS7[] =   {0x0A,0x08,0x08,0x08,0x00,0x00},
                   iERS7[] =   {0x04,0x02,0x04,0x00,0x00,0x00},
                   dERS7[] =   {0x02,0x04,0x02,0x04,0x00,0x00};
  const int* pp = isERS210 ? pERS210 : pERS7,
           * ii = isERS210 ? iERS210 : iERS7,
           * dd = isERS210 ? dERS210 : dERS7;

  for(int j = 0; j < 6; ++j)
  {
    if ( set[j] )
  {
    p[j] = pp[j];
    i[j] = ii[j];
    d[j] = dd[j];
  }
  }

}

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;
}

---