Skip to content

Commit

Permalink
Fixed vector to wheel actuation in footbot examples.
Browse files Browse the repository at this point in the history
  • Loading branch information
Carlo Pinciroli committed Nov 29, 2017
1 parent f738318 commit b527a14
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 32 deletions.
35 changes: 19 additions & 16 deletions controllers/footbot_flocking/footbot_flocking.cpp
View file
Expand Up @@ -202,22 +202,28 @@ void CFootBotFlocking::SetWheelSpeedsFromVector(const CVector2& c_heading) {
Real fHeadingLength = c_heading.Length();
/* Clamp the speed so that it's not greater than MaxSpeed */
Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);

/* Turning state switching conditions */
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
/* No Turn, heading angle very small */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
/* State transition logic */
if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::HARD_TURN) {
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
/* Hard Turn, heading angle very large */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::SOFT_TURN) {
if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
}
}
else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&
Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
/* Soft Turn, heading angle in between the two cases */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN) {
if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.NoTurnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
}

/* Wheel speeds based on current turning state */
Real fSpeed1, fSpeed2;
switch(m_sWheelTurningParams.TurningMechanism) {
Expand All @@ -227,23 +233,20 @@ void CFootBotFlocking::SetWheelSpeedsFromVector(const CVector2& c_heading) {
fSpeed2 = fBaseAngularWheelSpeed;
break;
}

case SWheelTurningParams::SOFT_TURN: {
/* Both wheels go straight, but one is faster than the other */
Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;
fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
break;
}

case SWheelTurningParams::HARD_TURN: {
/* Opposite wheel speeds */
fSpeed1 = -m_sWheelTurningParams.MaxSpeed;
fSpeed2 = m_sWheelTurningParams.MaxSpeed;
break;
}
}

/* Apply the calculated speeds to the appropriate wheels */
Real fLeftWheelSpeed, fRightWheelSpeed;
if(cHeadingAngle > CRadians::ZERO) {
Expand Down
35 changes: 19 additions & 16 deletions controllers/footbot_foraging/footbot_foraging.cpp
View file
Expand Up @@ -274,22 +274,28 @@ void CFootBotForaging::SetWheelSpeedsFromVector(const CVector2& c_heading) {
Real fHeadingLength = c_heading.Length();
/* Clamp the speed so that it's not greater than MaxSpeed */
Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);

/* Turning state switching conditions */
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
/* No Turn, heading angle very small */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
/* State transition logic */
if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::HARD_TURN) {
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
/* Hard Turn, heading angle very large */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::SOFT_TURN) {
if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
}
}
else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&
Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
/* Soft Turn, heading angle in between the two cases */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN) {
if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.NoTurnAngleThreshold) {
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
}

/* Wheel speeds based on current turning state */
Real fSpeed1, fSpeed2;
switch(m_sWheelTurningParams.TurningMechanism) {
Expand All @@ -299,23 +305,20 @@ void CFootBotForaging::SetWheelSpeedsFromVector(const CVector2& c_heading) {
fSpeed2 = fBaseAngularWheelSpeed;
break;
}

case SWheelTurningParams::SOFT_TURN: {
/* Both wheels go straight, but one is faster than the other */
Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;
fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
break;
}

case SWheelTurningParams::HARD_TURN: {
/* Opposite wheel speeds */
fSpeed1 = -m_sWheelTurningParams.MaxSpeed;
fSpeed2 = m_sWheelTurningParams.MaxSpeed;
break;
}
}

/* Apply the calculated speeds to the appropriate wheels */
Real fLeftWheelSpeed, fRightWheelSpeed;
if(cHeadingAngle > CRadians::ZERO) {
Expand Down

0 comments on commit b527a14

Please sign in to comment.