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

#include "core/MADevice.hpp"
#include "types/MABodyMotion.hpp"
#include "types/MARobotState.hpp"
#include "types/MATorso.hpp"
#include "types/MATypeRanges.hpp"

#include <MCLog.hpp>

#include <boost/math/constants/constants.hpp>

namespace MA
{
MCThreadLocalData<MATorsoController> Torso(false);
}

MATorsoController::MATorsoController() : MAController(MA::OtherController)
{
  if (MA::Torso.get())
  {
    MC_WARNING("The global torso controller is overridden.");
  }
  MA::Torso.reset(this);
}


MATorsoController::~MATorsoController()
{
  if (MA::Torso.get() == this)
    MA::Torso.release();
}


void MATorsoController::UpdateRobotState(MARobotState& state)
{
  float Pi = boost::math::constants::pi<float>();

  state.Torso->BackButtonF = (int)BackFButton->GetSensorValue();
  state.Torso->BackButtonM = (int)BackMButton->GetSensorValue();
  state.Torso->BackButtonR = (int)BackRButton->GetSensorValue();
  state.BodyMotion->AccelX.Raw = (int)(AccelX->GetSensorValue() / 100.0);
  SetAccelRawHighresValues(state.BodyMotion->AccelX.Raw, *AccelX);
  state.BodyMotion->AccelY.Raw = (int)(AccelY->GetSensorValue() / 100.0);
  SetAccelRawHighresValues(state.BodyMotion->AccelY.Raw, *AccelY);
  state.BodyMotion->AccelZ.Raw = (int)(AccelZ->GetSensorValue() / 100.0);
  SetAccelRawHighresValues(state.BodyMotion->AccelZ.Raw, *AccelZ);
  state.BodyMotion->AccelX.Degree = (int)(atan(AccelX->GetSensorValue() / 1000000.0)*180.0 / Pi);
  SetAccelDegreeHighresValues(state.BodyMotion->AccelX.Degree, *AccelX);
  state.BodyMotion->AccelY.Degree = (int)(atan(AccelY->GetSensorValue() / 1000000.0)*180.0 / Pi);
  SetAccelDegreeHighresValues(state.BodyMotion->AccelY.Degree, *AccelY);
  state.BodyMotion->AccelZ.Degree = (int)(atan(AccelZ->GetSensorValue() / 1000000.0)*180.0 / Pi);
  SetAccelDegreeHighresValues(state.BodyMotion->AccelZ.Degree, *AccelZ);
  state.Torso->ChestIR = (int)InfraChest->GetSensorValue();
  state.Torso->NearIR = (int)InfraNear->GetSensorValue();
  state.Torso->FarIR = (int)InfraFar->GetSensorValue();
  state.Torso->FreeMemory = GetFreeMemory();
}


MA::DeviceGeneratorsMap MATorsoController::GetSkitTransitionGenerators(MA::SkitBaseSPtr)
{
  return MA::DeviceGeneratorsMap();
}


void MATorsoController::SetAccelRawHighresValues(MANum<int>& value, MADevice& device) const
{
  MC::IntList NewValues;
  const MC::FloatList& OriginalValues = device.GetRealSensorValues();

  for (auto& value : OriginalValues)
  {
    NewValues.push_back((int)(value / 100));
  }
  value.SetHighResolutionValues(NewValues);
}


void MATorsoController::SetAccelDegreeHighresValues(MANum<int>& value, MADevice& device) const
{
  MC::IntList NewValues;
  const MC::FloatList& OriginalValues = device.GetRealSensorValues();

  for (auto& value : OriginalValues)
  {
    NewValues.push_back((int)(atan((value)/ 1000000.0)*180.0 / boost::math::constants::pi<float>()));
  }
  value.SetHighResolutionValues(NewValues);
}


MA::DeviceList MATorsoController::GetMotors() const
{
  return MA::DeviceList();
}


void MATorsoController::RequestRestart()
{
  // Nothing to do here
}


void MATorsoController::RequestShutdown()
{
  // Nothing to do here
}


void MATorsoController::RequestDebugMode()
{
  // Nothing to do here
}


int MATorsoController::GetFreeMemory() const
{
  // Nothing to do here
  return 0;
}