MALegMoveAway.cpp

/*
 *  This file is part of the AiBO+ project
 *
 *  Copyright (C) 2005-2016 Csaba Kertész (csaba.kertesz@gmail.com)
 *
 *  AiBO+ is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  AiBO+ is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Street #330, Boston, MA 02111-1307, USA.
 *
 */

#include "MALegMoveAway.hpp"

#include "types/MABodyMotion.hpp"
#include "types/MARobotState.hpp"
#include "controllers/MALegController.hpp"
#include "MALegLyingPosition.hpp"
#include "MALegSittingPosition.hpp"

MALegMoveAway::MALegMoveAway(MA::Leg::LegTypes leg_type) :
  MALegBehavior(MCGetClassName(this), leg_type)
{
  // Register the desired connections
  DesiredConnections[MCGetClassName<MALegLyingPosition>()] = -1.0;
  DesiredConnections[MCGetClassName<MALegSittingPosition>()] = -1.0;
}


MABehavior::StimulusLevelType MALegMoveAway::GetCurrentStimulus()
{
  // It is over-stimulated all the time
  return MABehavior::OverStimulated;
}


void MALegMoveAway::ActivatingActions()
{
  // Enable these cases only for lying position
  if (MA::RobotState->BodyMotion->Lying > 60)
  {
    // Special case for a fore leg: A fore leg is backward
    if ((LegState.Type == MA::Leg::LF || LegState.Type == MA::Leg::RF) &&
         LegState.IsBackwardNearBody())
    {
      StartTransition(*Controller, Controller->MoveAwayBackwardStr);
    } else
    // Special case for a hind leg: A fore leg is backward and the hind leg is forward above it
    if ((LegState.Type == MA::Leg::LH && LegState.IsForwardNearBody() &&
         MA::RobotState->LegLF->IsBackwardNearBody()) ||
        (LegState.Type == MA::Leg::RH && LegState.IsForwardNearBody() &&
               MA::RobotState->LegRF->IsBackwardNearBody()))
    {
      StartTransition(*Controller, Controller->MoveAwayFirstUpStr);
    } else {
      StartTransition(*Controller, Controller->MoveAwayStr);
    }
    return;
  }
  StartTransition(*Controller, Controller->MoveAwayStr);
}


void MALegMoveAway::FinishingActions()
{
  if (MA::RobotState->BodyMotion->Lying > 80)
  {
    MALEGBEHAVIOR_CREATE(MALegLyingPosition, LegState.Type, GetMaster());
  }
  if (MA::RobotState->BodyMotion->Sitting > 80)
  {
    // Switch the joints of the first legs back before moving the hind legs
    // TODO: Enable power savings
//    MA::LegLF->SetPower(MA::LegJoint1, true);
//    MA::LegRF->SetPower(MA::LegJoint1, true);
    MALEGBEHAVIOR_CREATE(MALegSittingPosition, LegState.Type, GetMaster());
  }
}