MATailController.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 "MATailController.hpp"

#include "core/MADevice.hpp"
#include "core/MAValueGenerators.hpp"
#include "types/skits/MAMotionSkit.hpp"
#include "types/MARobotState.hpp"
#include "types/MATypeRanges.hpp"
#include "types/MATorso.hpp"

#include <MCLog.hpp>

namespace MA
{
MCThreadLocalData<MATailController> Tail(false);
}

MATailController::MATailController() : MAController(MA::MotorController)
{
  if (MA::Tail.get())
  {
    MC_WARNING("The global tail controller is overridden.");
  }
  MA::Tail.reset(this);
  MA_BIND_TRANSITION_ACTION(MATailController, Wag);
  MA_BIND_TRANSITION(MATailController, ToBack, IsAtBack);
}


MATailController::~MATailController()
{
  if (MA::Tail.get() == this)
    MA::Tail.release();
}


void MATailController::UpdateRobotState(MARobotState& state)
{
  state.Torso->TailPan = (int)TailPan->GetSensorValue();
  state.Torso->TailPanForce = (int)TailPan->GetForce();
  state.Torso->TailTilt = (int)TailTilt->GetSensorValue();
  state.Torso->TailTiltForce = (int)TailTilt->GetForce();
}


MA::DeviceList MATailController::GetMotors() const
{
  MA::DeviceList Motors;

  Motors.push_back(TailTilt.get());
  Motors.push_back(TailPan.get());
  return Motors;
}


void MATailController::SetPower(bool new_state)
{
  // Clear the generators when switching off the joints
  if (!new_state)
    Stop();

  TailTilt->SetPower(new_state);
  TailPan->SetPower(new_state);
}


bool MATailController::IsPowerOn()
{
  return TailTilt->IsPowerOn() && TailPan->IsPowerOn();
}


bool MATailController::IsMoving()
{
  return TailTilt->HasGenerator() || TailPan->HasGenerator();
}


void MATailController::Stop()
{
  TailPan->ClearGenerators();
  TailTilt->ClearGenerators();
}


MA::DeviceGeneratorsMap MATailController::Wag()
{
  MA::DeviceGeneratorsMap DeviceGenerators;

  SetPower(true);
  // Move close to the body
  MASimpleGenerator* Generator = nullptr;
  float OneThird = (float)MA::TailTiltMin+(float)(MA::TailTiltMax-MA::TailTiltMin) / 3;

  TailTilt->ClearGenerators();
  Generator = new MASimpleGenerator(TailTilt->GetSensorValue(), (float)MA::TailTiltMin, 200);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), TailTilt->AddGenerator(*Generator));

  TailPan->ClearGenerators();
  Generator = new MASimpleGenerator(TailPan->GetSensorValue(), 0.0, 200);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailPan.get(), TailPan->AddGenerator(*Generator));

  // Start to wag the tail
  MAPeriodicGenerator* Generator2 = nullptr;

  Generator2 = new MAPeriodicGenerator((float)MA::TailTiltMin, OneThird, 0, 200, 0, 200, 0);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), TailTilt->AddGenerator(*Generator2));
  Generator2 = new MAPeriodicGenerator((float)MA::TailTiltMin, OneThird, 0, 200, 0, 200, 0);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), TailTilt->AddGenerator(*Generator2));
  Generator2 = new MAPeriodicGenerator((float)MA::TailTiltMin, OneThird, 0, 200, 0, 200, 0);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), TailTilt->AddGenerator(*Generator2));
  Generator2 = new MAPeriodicGenerator((float)MA::TailTiltMin, OneThird, 0, 200, 0, 200, 0);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), TailTilt->AddGenerator(*Generator2));
  return DeviceGenerators;
}


MA::DeviceGeneratorsMap MATailController::ToBack()
{
  MASimpleGenerator* Generator = nullptr;
  MA::DeviceGeneratorsMap DeviceGenerators;

  SetPower(true);
  TailTilt->ClearGenerators();
  Generator = new MASimpleGenerator(TailTilt->GetSensorValue(), (float)MA::TailTiltMin, 500);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), TailTilt->AddGenerator(*Generator));

  TailPan->ClearGenerators();
  Generator = new MASimpleGenerator(TailPan->GetSensorValue(), 0.0, 500);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailPan.get(), TailPan->AddGenerator(*Generator));
  return DeviceGenerators;
}


bool MATailController::IsAtBack() const
{
  bool Ret = true;

  Ret = Ret && MCRangeCheck((int)TailPan->GetSensorValue(), -20, 20);
  Ret = Ret && MCRangeCheck((int)TailTilt->GetSensorValue(), -20, 20);
  return Ret;
}


MA::DeviceGeneratorsMap MATailController::GetSkitTransitionGenerators(MA::SkitBaseSPtr skit)
{
  MAMotionSkit* MotionSkit = dynamic_cast<MAMotionSkit*>(skit.get());

  if (!MotionSkit)
    return MA::DeviceGeneratorsMap();

  MA::DeviceGeneratorsMap DeviceGenerators;
  MAGeneratorContainer* Container = nullptr;

  SetPower(true);
  // Tilt
  Container = MotionSkit->GetGeneratorContainer(MA::TailTilt);
  TailTilt->ClearGenerators();
  TailTilt->AddGeneratorContainer(*Container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailTilt.get(), Container);
  // Pan
  Container = MotionSkit->GetGeneratorContainer(MA::TailPan);
  TailPan->ClearGenerators();
  TailPan->AddGeneratorContainer(*Container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, TailPan.get(), Container);
  return DeviceGenerators;
}