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

# Representations/Motion/PIDData.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file PIDData.cpp
    00003 *
    00004 * Implementation of class PIDData
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #include "PIDData.h"
    00010 #include "Platform/SystemCall.h"
    00011 #include "Tools/RobotConfiguration.h"
    00012
    00013 PIDData::PIDData()
    00014 {
    00015   setToDefaults();
    00016 }
    00017
    00018 PIDData& PIDData::operator=(const PIDData& other)
    00019 {
    00020   for (int j = 0; j < JointData::numOfJoint; j++)
    00021   {
    00022     p[j] = other.p[j];
    00023     i[j] = other.i[j];
    00024     d[j] = other.d[j];
    00025   }
    00026   return *this;
    00027 }
    00028
    00029 PIDData::PIDData(const PIDData& other)
    00030 {
    00031   *this = other;
    00032 }
    00033
    00034 void PIDData::setHeadToDefaults(bool neckTilt, bool headPan, bool headTilt, bool mouth, bool earL, bool earR)
    00035 {
    00036   const bool set[] = { neckTilt, headPan,headTilt,mouth,earL,earR};
    00037   bool isERS210 = getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210;
    00038   static const int pERS210[] = {0x0A,0x0D,0x0A,0x0E,0x00,0x00},
    00039                    iERS210[] = {0x08,0x08,0x08,0x08,0x00,0x00},
    00040                    dERS210[] = {0x0C,0x0B,0x0C,0x10,0x00,0x00},
    00041                    pERS7[] =   {0x0A,0x08,0x08,0x08,0x00,0x00},
    00042                    iERS7[] =   {0x04,0x02,0x04,0x00,0x00,0x00},
    00043                    dERS7[] =   {0x02,0x04,0x02,0x04,0x00,0x00};
    00044   const int* pp = isERS210 ? pERS210 : pERS7,
    00045            * ii = isERS210 ? iERS210 : iERS7,
    00046            * dd = isERS210 ? dERS210 : dERS7;
    00047
    00048   for(int j = 0; j < 6; ++j)
    00049   {
    00050     if ( set[j] )
    00051   {
    00052     p[j] = pp[j];
    00053     i[j] = ii[j];
    00054     d[j] = dd[j];
    00055   }
    00056   }
    00057
    00058 }
    00059
    00060 void PIDData::setToDefaults()
    00061 {
    00062   bool isERS210 = getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210;
    00063   static const int pERS210[] = {0x0A,0x0D,0x0A,0x0E,0x00,0x00,0x16,0x14,0x23,0x16,0x14,0x23,0x16,0x14,0x23,0x16,0x14,0x23,0x0A,0x07},
    00064                    iERS210[] = {0x08,0x08,0x08,0x08,0x00,0x00,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x00,0x00},
    00065                    dERS210[] = {0x0C,0x0B,0x0C,0x10,0x00,0x00,0x08,0x06,0x05,0x08,0x06,0x05,0x08,0x06,0x05,0x08,0x06,0x05,0x18,0x11},
    00066                    pERS7[] =   {0x0A,0x08,0x08,0x08,0x00,0x00,0x1C,0x14,0x1C,0x1C,0x14,0x1C,0x1C,0x14,0x1C,0x1C,0x14,0x1C,0x0A,0x0A},
    00067                    iERS7[] =   {0x04,0x02,0x04,0x00,0x00,0x00,0x08,0x04,0x08,0x08,0x04,0x08,0x08,0x04,0x08,0x08,0x04,0x08,0x04,0x04},
    00068                    dERS7[] =   {0x02,0x04,0x02,0x04,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x04,0x04};
    00069   const int* pp = isERS210 ? pERS210 : pERS7,
    00070            * ii = isERS210 ? iERS210 : iERS7,
    00071            * dd = isERS210 ? dERS210 : dERS7;
    00072
    00073   for(int j = 0; j < JointData::numOfJoint; ++j)
    00074   {
    00075     p[j] = pp[j];
    00076     i[j] = ii[j];
    00077     d[j] = dd[j];
    00078   }
    00079 }
    00080
    00081 void PIDData::setValues(JointData::JointID joint, int ap, int ai, int ad)
    00082 {
    00083   p[joint] = ap;
    00084   i[joint] = ai;
    00085   d[joint] = ad;
    00086 }
    00087
    00088 void PIDData::setLegFJ1Values(int ap, int ai, int ad)
    00089 {
    00090   setValues(JointData::legFR1, ap, ai, ad);
    00091   setValues(JointData::legFL1, ap, ai, ad);
    00092 }
    00093
    00094 void PIDData::setLegHJ1Values(int ap, int ai, int ad)
    00095 {
    00096   setValues(JointData::legHR1, ap, ai, ad);
    00097   setValues(JointData::legHL1, ap, ai, ad);
    00098 }
    00099
    00100 void PIDData::setLegFJ2Values(int ap, int ai, int ad)
    00101 {
    00102   setValues(JointData::legFR2, ap, ai, ad);
    00103   setValues(JointData::legFL2, ap, ai, ad);
    00104 }
    00105
    00106 void PIDData::setLegHJ2Values(int ap, int ai, int ad)
    00107 {
    00108   setValues(JointData::legHR2, ap, ai, ad);
    00109   setValues(JointData::legHL2, ap, ai, ad);
    00110 }
    00111
    00112 void PIDData::setLegFJ3Values(int ap, int ai, int ad)
    00113 {
    00114   setValues(JointData::legFR3, ap, ai, ad);
    00115   setValues(JointData::legFL3, ap, ai, ad);
    00116 }
    00117
    00118 void PIDData::setLegHJ3Values(int ap, int ai, int ad)
    00119 {
    00120   setValues(JointData::legHR3, ap, ai, ad);
    00121   setValues(JointData::legHL3, ap, ai, ad);
    00122 }
    00123
    00124 In& operator>>(In& stream,PIDData& pidData)
    00125 {
    00126   for (int i = 0; i < JointData::numOfJoint; i++)
    00127   {
    00128     stream >> pidData.p[i];
    00129     stream >> pidData.i[i];
    00130     stream >> pidData.d[i];
    00131   }
    00132   return stream;
    00133 }
    00134
    00135 Out& operator<<(Out& stream, const PIDData& pidData)
    00136 {
    00137   for (int i = 0; i < JointData::numOfJoint; i++)
    00138   {
    00139     stream << pidData.p[i];
    00140     stream << pidData.i[i];
    00141     stream << pidData.d[i];
    00142   }
    00143   return stream;
    00144 }