MALegController.hpp

/*
 *  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.
 *
 */

#pragma once

#include "types/MABodyTypes.hpp"
#include "types/MALeg.hpp"
#include "MAController.hpp"

#include <MCThreadLocalData.hpp>

#include <boost/shared_ptr.hpp>

class MADevice;
class MAGeneratorBase;
class MALegController;
class MARobotState;

namespace MA
{
extern MCThreadLocalData<MALegController> LegLF;
extern MCThreadLocalData<MALegController> LegLH;
extern MCThreadLocalData<MALegController> LegRF;
extern MCThreadLocalData<MALegController> LegRH;
}

struct MALegTransitionGenerators
{
  MALegTransitionGenerators() : Joint1(nullptr), Joint2(nullptr), Joint3(nullptr)
  { }

  void AddGeneratorToJoint1(MAGeneratorBase& generator);
  void AddGeneratorToJoint2(MAGeneratorBase& generator);
  void AddGeneratorToJoint3(MAGeneratorBase& generator);
  void AlignDurations();

  MAGeneratorContainer* Joint1;
  MAGeneratorContainer* Joint2;
  MAGeneratorContainer* Joint3;
};

class MALegController : public MAController
{
public:
  explicit MALegController(MA::Leg::LegTypes leg);
  virtual ~MALegController();

private:
  virtual void UpdateRobotState(MARobotState& state) override;

public:
  virtual MA::DeviceList GetMotors() const override;
  void SetPower(MA::LegJointsType joint, bool new_state);
  void SetPower(bool new_state);
  bool IsPowerOn(MA::LegJointsType joint) const;
  bool IsPowerOn() const;
  bool IsMoving() const;
  void Stop();
  MA_DECLARE_TRANSITION(Pull, IsPulled)
  MA_DECLARE_TRANSITION(SwingForTurnOver, IsSwungForTurnOver)
  MA_DECLARE_TRANSITION(ToLyingPosition, IsLyingPosition)
  MA_DECLARE_TRANSITION(MoveAway, IsMovedAway)
  MA_DECLARE_TRANSITION_ACTION(MoveAwayBackward)
  MA_DECLARE_TRANSITION_ACTION(MoveAwayFirstUp)
  MA_DECLARE_TRANSITION(ToSittingPosition, IsSittingPosition)
  MA_DECLARE_TRANSITION_ACTION(WalkSwingForward)
  MA_DECLARE_TRANSITION_ACTION(WalkPushBackward)
  MA_DECLARE_TRANSITION_ACTION(WalkSwingBackward)
  MA_DECLARE_TRANSITION_ACTION(WalkPushForward)
  MA_DECLARE_TRANSITION(ToInitialWalkPosition, IsInitialWalkPosition)
  MA_DECLARE_TRANSITION_ACTION(BalanceToCenter)
  MA_DECLARE_TRANSITION_ACTION(BalanceToLeft)
  MA_DECLARE_TRANSITION_ACTION(BalanceToRight)
  MA_DECLARE_TRANSITION_ACTION(AdjustStandingPosture)
  bool IsAhead() const;
  bool IsBack() const;
  void PrintFeatures();

protected:
  MADevice* GetDevice(const MABodyPartFeature& body_part) const;
  void RegisterDeviceUpdates();
  void NotifyDeviceUpdate(MADevice& device);
  void CreateGenerators(MALegTransitionGenerators& generators, const MABodyPartFeature& target, int msec);
  virtual MA::DeviceGeneratorsMap GetSkitTransitionGenerators(MA::SkitBaseSPtr skit) override;
  MA::DeviceGeneratorsMap ApplyGenerators(MALegTransitionGenerators& generators, bool clear = false);

public:
  inline MA::Leg::LegTypes GetLegType() const
  {
    return LegType;
  }

protected:
  MA::Leg::LegTypes LegType;
  boost::shared_ptr<MADevice> Joint1;
  boost::shared_ptr<MADevice> Joint2;
  boost::shared_ptr<MADevice> Joint3;
  boost::shared_ptr<MADevice> Paw;
  /*
   * Forward-backward and upward-downward direction
   * -100 - backward 100 - forward
   * -100 - downward 100 - upward
   */
  MALegPosition Position;
  /*
   * Sidelong direction
   * 0 - least 100 - full
   */
  MASidelong Sidelong;
  /*
   * Bent-stretched state
   * 0 - full stretched 100 - full bent
   */
  MABentStretched BentStretched;
};