Commit 8467d216 authored by NoelieRamuzat's avatar NoelieRamuzat Committed by Olivier Stasse
Browse files

[Phase Signal] Add enum and private variable dedicated to phase (int)

parent 8295f29e
......@@ -62,6 +62,13 @@ namespace pg = PatternGeneratorJRL;
namespace dynamicgraph {
namespace sot {
/// Define the support phase of the robot
enum SupportPhase {
DOUBLE_SUPPORT_PHASE = 0,
LEFT_SUPPORT_PHASE = 1,
RIGHT_SUPPORT_PHASE = -1,
};
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -170,6 +177,8 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
/*! \brief Double support phase detected. */
bool m_DoubleSupportPhaseState;
/*! \brief Current contact phase defined by enum: leftFoot=1, rightFoot=-1, doubleSupport=0. */
SupportPhase m_ContactPhase;
int m_DSStartingTime;
/*! \brief iteration time. */
......@@ -503,7 +512,7 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
bool &getLeftFootContact(bool &res, int time);
bool &getRightFootContact(bool &res, int time);
/*! \brief Internal method to get the information of contact phase leftFoot=1, rightFoot=-1, doubleSupport=0. */
double &getContactPhase(double &res, int time);
int &getContactPhase(int &res, int time);
public:
/*! \name External signals
......@@ -630,8 +639,8 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
SignalTimeDependent<bool, int> rightFootContactSOUT;
/*! @} */
/*! \brief Int Vector of contact phase: leftFoot=1, rightFoot=-1, doubleSupport=0. */
SignalTimeDependent<double, int> contactPhaseSOUT;
/*! \brief Int Vector of support phase: leftFoot=1, rightFoot=-1, doubleSupport=0 (see enum). */
SignalTimeDependent<int, int> contactPhaseSOUT;
/*! \name Reimplement the interface of the plugin.
@{ */
......
......@@ -309,7 +309,7 @@ PatternGenerator::PatternGenerator(const std::string &name)
contactPhaseSOUT(
boost::bind(&PatternGenerator::getContactPhase, this, _1, _2),
OneStepOfControlS,
"PatternGenerator(" + name + ")::output(double)::contactphase")
"PatternGenerator(" + name + ")::output(int)::contactphase")
{
m_MotionSinceInstanciationToThisSequence.setIdentity();
......@@ -318,6 +318,7 @@ PatternGenerator::PatternGenerator(const std::string &name)
m_count = 0;
m_TimeStep = 0.005;
m_DoubleSupportPhaseState = true;
m_ContactPhase = DOUBLE_SUPPORT_PHASE;
m_forceFeedBack = false;
m_feedBackControl = false;
......@@ -1013,16 +1014,10 @@ bool &PatternGenerator ::getRightFootContact(bool &res, int time) {
return res;
}
double &PatternGenerator ::getContactPhase(double &res, int time) {
int &PatternGenerator ::getContactPhase(int &res, int time) {
sotDEBUGIN(25);
OneStepOfControlS(time);
if ((m_leftFootContact) && !(m_DoubleSupportPhaseState)){
res = 1.0;
} else if ((m_rightFootContact) && !(m_DoubleSupportPhaseState)){
res = -1.0;
} else {
res = 0.0;
}
res = m_ContactPhase;
sotDEBUGOUT(25);
return res;
}
......@@ -1534,6 +1529,7 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
m_leftFootContact = true;
m_rightFootContact = true;
m_DoubleSupportPhaseState = true;
m_ContactPhase = DOUBLE_SUPPORT_PHASE;
}
if (lLeftFootPosition.stepType == -1) {
......@@ -1542,6 +1538,7 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
if ((lRightFootPosition.stepType != -1) && (lRightFootPosition.stepType != 10)){
m_rightFootContact = false;
m_DoubleSupportPhaseState = false;
m_ContactPhase = LEFT_SUPPORT_PHASE;
}
} else if (lRightFootPosition.stepType == -1) {
lSupportFoot = 0;
......@@ -1549,6 +1546,7 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
if ((lLeftFootPosition.stepType != -1) && (lLeftFootPosition.stepType != 10)) {
m_leftFootContact = false;
m_DoubleSupportPhaseState = false;
m_ContactPhase = RIGHT_SUPPORT_PHASE;
}
} else
/* m_LeftFootPosition.z ==m_RightFootPosition.z
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment