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

#include "MAAudio.hpp"
#include "MABodyMotion.hpp"
#include "MARobotState.hpp"
#include "MAComplexIndicators.hpp"
#include "MATorso.hpp"

MAGoals::MAGoals() : MARobotStateUpdater(MA::UpdateComplexIndicators),
  DesiredForwardWalkDuration(0, 0, 60000), DesiredBackwardWalkDuration(0, 0, 60000),
  DesiredLeftTurnDuration(0, 0, 40000), DesiredRightTurnDuration(0, 0, 40000),
  ArousalFeedbackNeeded(0, 0, 10000), ListenToAudioDesire(0, 0, 10000),
  DiscoverSoundSource(0, 0, 2000), DesiredAudioCaptureDuration(0, 0, 5000)
{
}


bool MAGoals::IsLocomotionDesired() const
{
  return DesiredForwardWalkDuration > 0 || DesiredBackwardWalkDuration > 0 ||
         DesiredLeftTurnDuration > 0 || DesiredRightTurnDuration > 0;
}


void MAGoals::ResetLocomotionGoals()
{
  LiePostureDesire = 0;
  SitPostureDesire = 0;
  StandPostureDesire = 0;
  DesiredForwardWalkDuration = 0;
  DesiredBackwardWalkDuration = 0;
  DesiredLeftTurnDuration = 0;
  DesiredRightTurnDuration = 0;
}


void MAGoals::UpdateRobotState(MARobotState& state)
{
  ArousalFeedbackNeeded -= state.Torso->CycleTime;
  // Correct any wrong desires if there is an abyss before the dog while lying
  if (state.ComplexIndicators->AbyssWhileLying == 1)
  {
     SitPostureDesire = 0;
     StandPostureDesire = 0;
     DesiredForwardWalkDuration = 0;
     DesiredBackwardWalkDuration = 0;
     DesiredLeftTurnDuration = 0;
     DesiredRightTurnDuration = 0;
  }
  // Stop the motion control if there is no new command received in 60 seconds
  if (state.BodyMotion->MotionControlAge == 60)
  {
    state.BodyMotion->StopMotionControlTimer();
    MotionControl = 0;
  }
  if (state.BodyMotion->MotionControlAge > 0 && MotionControl == 0)
  {
    state.BodyMotion->StopMotionControlTimer();
  }
  GenerateHeadTurnDesires(state);
  GenerateDiscoverSoundSource(state);
}


void MAGoals::GenerateHeadTurnDesires(MARobotState& state)
{
  if (DiscoverSoundSource == 0)
  {
    TurnHeadToRight = 0;
    TurnHeadToLeft = 0;
    return;
  }

  const int HeadLeftRightLimit = 85;

  if (state.ComplexIndicators->LeftMicrophoneDominance > state.ComplexIndicators->RightMicrophoneDominance &&
      state.ComplexIndicators->LeftMicrophoneDominance > state.ComplexIndicators->MicrophoneChannelsBalance &&
      state.Head->LeftRight > -HeadLeftRightLimit)
  {
    TurnHeadToLeft = 1;
  } else {
    TurnHeadToLeft = 0;
  }
  if (state.ComplexIndicators->RightMicrophoneDominance > state.ComplexIndicators->LeftMicrophoneDominance &&
      state.ComplexIndicators->RightMicrophoneDominance > state.ComplexIndicators->MicrophoneChannelsBalance &&
      state.Head->LeftRight < HeadLeftRightLimit)
  {
    TurnHeadToRight = 1;
  } else {
    TurnHeadToRight = 0;
  }
}


void MAGoals::GenerateDiscoverSoundSource(MARobotState& state)
{
  DiscoverSoundSource -= state.Torso->CycleTime;
}