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

#include "MAAudio.hpp"
#include "MABodyMotion.hpp"
#include "MAGoals.hpp"
#include "MAHead.hpp"
#include "MALeg.hpp"
#include "MARobotState.hpp"
#include "MATorso.hpp"
#include "MATypeRanges.hpp"

MAComplexIndicators::MAComplexIndicators() : MARobotStateUpdater(MA::UpdateComplexIndicators),
  WalkPeriod(2400, 1400, 3200),
  ElapsedForwardWalkTime(0, 0, 100000),
  ElapsedBackwardWalkTime(0, 0, 100000),
  ElapsedLeftTurnTime(0, 0, 20700),
  ElapsedRightTurnTime(0, 0, 20700),
  AbyssWhileLying(-1, -1, 1),
  LeftMicrophoneDominance(0, 0, 255), RightMicrophoneDominance(0, 0, 255),
  MicrophoneChannelsBalance(0, 0, 255),
  NearObjectForwardFactor(0, 0, 2000),
  AbyssWhileWalkingFactor(0, 0, 3000),
  BodyOutOfWalkingRangeFactor(0, 0, 500),
  OverloadDangerWhileSittingFactor(0, 0, 3000),
  WalkSpeedAccelerationStart(-1)
{
  AbyssWhileWalking.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  NearObjectForward.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BodyOutOfWalkingRange.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  OverloadDangerWhileSitting.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
}


bool MAComplexIndicators::IsLocomotionActive() const
{
  return ElapsedBackwardWalkTime > 0 || ElapsedForwardWalkTime > 0 || ElapsedLeftTurnTime > 0 ||
         ElapsedRightTurnTime > 0;
}


void MAComplexIndicators::UpdateRobotState(MARobotState& state)
{
  IsLyingPosture = state.LegLF->IsLyingPosition &&
                   state.LegLH->IsLyingPosition &&
                   state.LegRF->IsLyingPosition &&
                   state.LegRH->IsLyingPosition;
  IsStandingPosture = state.LegLF->IsStandingPosition &&
                      state.LegLH->IsStandingPosition &&
                      state.LegRF->IsStandingPosition &&
                      state.LegRH->IsStandingPosition;
  IsSittingPosture = state.LegLF->IsSittingPosition &&
                     state.LegLH->IsSittingPosition &&
                     state.LegRF->IsSittingPosition &&
                     state.LegRH->IsSittingPosition;
  GenerateWalkPeriod(state);
  GenerateNearObjectForward(state);
  GenerateAbyssWhileWalking(state);
  GenerateAbyssWhileLying(state);
  GenerateBodyOutOfWalkingRange(state);
  GenerateOverloadDangerWhileSitting(state);
  GenerateListenWithEars(state);
  GenerateMicrophoneChannelDominances(state);
}


void MAComplexIndicators::GenerateWalkPeriod(MARobotState& state)
{
  if ((ElapsedForwardWalkTime == 0 && ElapsedForwardWalkTime.GetValueChange() != 0) ||
      (ElapsedBackwardWalkTime == 0 && ElapsedBackwardWalkTime.GetValueChange() != 0))
  {
    WalkSpeedAccelerationStart = -1;
    WalkPeriod = 2400;
  } else
  if (ElapsedForwardWalkTime > 5600 || ElapsedBackwardWalkTime > 5600)
  {
    // Increase the forward walk speed by lowering the walk period to 2000
    if (ElapsedForwardWalkTime > 0 && state.BodyMotion->Floor.Surface != MA::UnknownFloor &&
        state.BodyMotion->Floor.Surface != MA::Carpet)
    {
      if (WalkSpeedAccelerationStart == -1)
      {
        WalkSpeedAccelerationStart = ElapsedForwardWalkTime;
      }
      if (ElapsedForwardWalkTime-WalkSpeedAccelerationStart < 4800)
      {
        WalkPeriod = 2400-(ElapsedForwardWalkTime-WalkSpeedAccelerationStart) / 12;
      } else {
        WalkPeriod = 2000;
      }
    } else
    {
      WalkSpeedAccelerationStart = -1;
      if (WalkPeriod > 2350)
        WalkPeriod = 2400;
    }
  } else // -V705 (PVS Studio suppression)
  // Adjust the walk period from 2800 to 2400 when the walk is beginning instead of
  // a sudden start with the same speed.
  if (ElapsedForwardWalkTime > 0)
  {
    WalkPeriod = 2800-ElapsedForwardWalkTime / 14;
    WalkSpeedAccelerationStart = -1;
  } else
  if (ElapsedBackwardWalkTime > 0)
  {
    WalkPeriod = 2800-ElapsedBackwardWalkTime / 14;
    WalkSpeedAccelerationStart = -1;
  }
}


void MAComplexIndicators::GenerateNearObjectForward(MARobotState& state)
{
  if (state.Torso->NearIR <= 15 && state.BodyMotion->IsBodyRollInWalkRange())
  {
    // Obstacle detection can be reached in 500 ms
    NearObjectForwardFactor += state.Torso->CycleTime*4;
  } else {
    NearObjectForwardFactor -= state.Torso->CycleTime;
  }
  NearObjectForward = NearObjectForwardFactor.GetScaledValue();
}


void MAComplexIndicators::GenerateAbyssWhileWalking(MARobotState& state)
{
  if (state.Torso->ChestIR > 65 && state.BodyMotion->IsBodyRollInWalkRange() &&
      // The activation starts while walking, but it is kept after the forward walk is stopped
      (ElapsedForwardWalkTime > 0 || AbyssWhileWalking > 0))
  {
    // Abyss detection can be reached in 500 ms
    AbyssWhileWalkingFactor += state.Torso->CycleTime*6;
  } else {
    AbyssWhileWalkingFactor -= state.Torso->CycleTime;
  }
  AbyssWhileWalking = AbyssWhileWalkingFactor.GetScaledValue();
}


void MAComplexIndicators::GenerateAbyssWhileLying(MARobotState& state)
{
  if (state.BodyMotion->Flying > 90 || state.BodyMotion->UpsideDown > 80 ||
      ElapsedBackwardWalkTime > 0 || ElapsedForwardWalkTime > 0 ||
      ElapsedLeftTurnTime > 0 || ElapsedRightTurnTime > 0)
  {
    AbyssWhileLying = -1;
    return;
  }
}


void MAComplexIndicators::GenerateBodyOutOfWalkingRange(MARobotState& state)
{
  if (!state.BodyMotion->IsBodyRollInWalkRange())
  {
    // The robot body can be out of walking range after 500 ms
    BodyOutOfWalkingRangeFactor += state.Torso->CycleTime;
  } else {
    BodyOutOfWalkingRangeFactor -= state.Torso->CycleTime;
  }
  BodyOutOfWalkingRange = BodyOutOfWalkingRangeFactor.GetScaledValue();
}


void MAComplexIndicators::GenerateOverloadDangerWhileSitting(MARobotState& state)
{
  int Hits = 0;

  Hits += state.LegLF->Force1 <= MA::ForeLegForce1SittingLimit;
  Hits += (state.LegLF->Force1 <= MA::ForeLegForce1SittingEmergencyLimit)*3;
  Hits += state.LegLF->Force3 <= MA::ForeLegForce3SittingLimit;
  Hits += state.LegRF->Force1 <= MA::ForeLegForce1SittingLimit;
  Hits += (state.LegRF->Force1 <= MA::ForeLegForce1SittingEmergencyLimit)*3;
  Hits += state.LegRF->Force3 <= MA::ForeLegForce3SittingLimit;
  if (Hits >= 3)
  {
    // The front legs are overloaded while sitting after 1500 ms
    OverloadDangerWhileSittingFactor += state.Torso->CycleTime;
  } else {
    OverloadDangerWhileSittingFactor -= state.Torso->CycleTime;
  }
  OverloadDangerWhileSitting = OverloadDangerWhileSittingFactor.GetScaledValue();
}


void MAComplexIndicators::GenerateListenWithEars(MARobotState& state)
{
  bool SelfNoises = false;

  SelfNoises = SelfNoises || (state.Audio->RemainingPlaybackTime > 0 && state.Audio->SpeakerVolume > 0);
  SelfNoises = SelfNoises || state.LegLF->InMotion > 0 || state.LegLH->InMotion > 0;
  SelfNoises = SelfNoises || state.LegRF->InMotion > 0 || state.LegRH->InMotion > 0;
  SelfNoises = SelfNoises || (state.Head->InMotion > 0 && !state.Goals->TurnHeadToLeft && !state.Goals->TurnHeadToRight);

  bool GoodPose = false;

  GoodPose = GoodPose || state.BodyMotion->Lying > 90;
  GoodPose = GoodPose || state.BodyMotion->Sitting > 90;
  GoodPose = GoodPose || IsStandingPosture > 0;

  if (!SelfNoises && GoodPose)
    ListenWithEars = 1;
  else
    ListenWithEars = 0;
}


void MAComplexIndicators::GenerateMicrophoneChannelDominances(MARobotState& state)
{
  const int MinPower = 2000;
  const int LeftPower = state.Audio->LeftMicrophonePower;
  const int RightPower = state.Audio->RightMicrophonePower;
  const int BackgroundPower = state.Audio->BackgroundSoundPower;

  if (((LeftPower > MinPower && LeftPower > BackgroundPower*5) ||
       (RightPower > MinPower && RightPower > BackgroundPower*5)) &&
      MA::RobotState->ComplexIndicators->ListenWithEars == 1)
  {
    if ((float)LeftPower > (float)RightPower*1.05)
      LeftMicrophoneDominance += 3;
    else
    if ((float)LeftPower*1.05 < (float)RightPower)
      RightMicrophoneDominance += 3;
    else
      MicrophoneChannelsBalance += 3;
  }
  // Similar to other indicators, the dominances are accumulated over time,
  // but decreased at fixed rate.
  LeftMicrophoneDominance -= 2;
  RightMicrophoneDominance -= 2;
  MicrophoneChannelsBalance -= 2;
}