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

#include "MARobotState.hpp"
#include "MATypeRanges.hpp"

#include <MCLog.hpp>

#include <limits.h>

MATimestamp::MATimestamp()
{
  Seconds.SetMinMax(0, INT_MAX);
  Milliseconds.SetMinMax(0, 999);
  Microseconds.SetMinMax(0, 999);
}


int MATimestamp::ToMilliseconds() const
{
  return Seconds*1000+Milliseconds;
}


MACpuLoad::MACpuLoad()
{
  All.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Behaviors.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Camera.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Network.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Audio.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
}


MAWlanStatistics::MAWlanStatistics()
{
  Link.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Signal.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Noise.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
}


MATorso::MATorso() : MARobotStateUpdater(MA::UpdateSensorIndicators),
  CycleID(0),
  CycleTime(0),
  WritableStorage(1, 0, 1),
  ActivatedStateTimer(MC::NoExpiration),
  BackFPressedFactor(0, 0, 1000),
  BackMPressedFactor(0, 0, 1000),
  BackRPressedFactor(0, 0, 1000),
  BackFTouched(MA::BackBtnMax),
  BackMTouched(MA::BackBtnMax),
  BackRTouched(MA::BackBtnMax),
  BackStrokedFactor(0, 0, 2000),
  BackGrabbedFactor(0, 0, 400)
{
  BuildID.SetMinMax(0, std::numeric_limits<int32_t>::max());
  SessionID.SetMinMax(0, std::numeric_limits<int32_t>::max());
  CycleID.SetMinMax(0, std::numeric_limits<int32_t>::max());
  // Maximum 32 seconds cycle time :)
  CycleTime.SetMinMax(0, 32000);
  FreeMemory.SetMinMax(0, std::numeric_limits<int32_t>::max());
  // Note: The official battery capacity is 2200 mAh, but who knows...
  BatteryCapacity.SetMinMax(0, 3200);
  BatteryLevel.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  PowerConsumption.SetMinMax(0, 32000);
  // Note: The precision is two decimals (see calculations in APServer::UpdateRobotState()).
  EstimatedSessionPowerConsumption.SetMinMax(0, 16280*100);
  EstimatedSessionPowerConsumption.SetMagnitude(100);
  // The design battery capacity (ERS-7) is 2200 mAh*7.4 V = 16280 mWh
  // Note: The 7.4 V should be multiplied by 10 to avoid decimals and by 100 again
  // because of the percentage based calculations in APServer::UpdateRobotState().
  RealSessionPowerConsumption.SetMinMax(0, 16280*10*100);
  RealSessionPowerConsumption.SetMagnitude(10*100);
  Temperature.SetMinMax(MA::TemperatureMin, MA::TemperatureMax);
  DataUpload.SetMinMax(0, std::numeric_limits<int32_t>::max());
  // Note: Maximum 32 seconds long audio playback
  TailPan.SetMinMax(MA::TailPanMin, MA::TailPanMax);
  TailPanForce.SetMinMax(MA::JointForceMin, MA::JointForceMax);
  TailTilt.SetMinMax(MA::TailTiltMin, MA::TailTiltMax);
  TailTiltForce.SetMinMax(MA::JointForceMin, MA::JointForceMax);
  BackButtonF.SetMinMax(MA::BackBtnMin, MA::BackBtnMax);
  BackButtonM.SetMinMax(MA::BackBtnMin, MA::BackBtnMax);
  BackButtonR.SetMinMax(MA::BackBtnMin, MA::BackBtnMax);
  ChestIR.SetMinMax(MA::IRChestMin, MA::IRChestMax);
  NearIR.SetMinMax(MA::IRNearMin, MA::IRNearMax);
  FarIR.SetMinMax(MA::IRFarMin, MA::IRFarMax);
  BackFButtonPressed.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BackMButtonPressed.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BackRButtonPressed.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BackStroked.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BackGrabbed.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  ConnectedClients.SetMinMax(0, 255);
}


void MATorso::UpdateCycleTime()
{
  // Create and start an activation timer if it has not been created yet.
  if (unlikely(!ActivatedStateTimer.IsStarted()))
  {
    ActivatedStateTimer.Start();
    CycleTime = 0;
    return;
  }
  // Cycle IDs from 1-
  ++CycleID;
  // Set the current activation base which is in fact the elapsed milliseconds
  // since the last update.
  CycleTime = ActivatedStateTimer.GetElapsedTime();
  // Restart the activation timer
  ActivatedStateTimer.Start();
}


void MATorso::UpdateTimestamp()
{
  int64_t Time = MCTimer::GetElapsedSystemTime();
  int MSecs = Time / 1000;

  Timestamp.Seconds = MSecs / 1000;
  Timestamp.Milliseconds = MSecs % 1000;
  Timestamp.Microseconds = Time % 1000;
}


void MATorso::UpdateRobotState(MARobotState& state)
{
  GenerateBackFButtonPressed(state.Torso->CycleTime);
  GenerateBackMButtonPressed(state.Torso->CycleTime);
  GenerateBackRButtonPressed(state.Torso->CycleTime);
  GenerateBackStroked(state.Torso->CycleTime);
  GenerateBackGrabbed(state.Torso->CycleTime);
}


void MATorso::GenerateBackFButtonPressed(int cycle_time)
{
  if (BackButtonF >= 13)
  {
    // Pressed "state" can be reached in 1 second
    BackFPressedFactor += cycle_time;
  } else {
    BackFPressedFactor -= cycle_time*3;
  }
  BackFButtonPressed = BackFPressedFactor.GetScaledValue();
}


void MATorso::GenerateBackMButtonPressed(int cycle_time)
{
  if (BackButtonM >= 13)
  {
    // Pressed "state" can be reached in 1 second
    BackMPressedFactor += cycle_time;
  } else {
    BackMPressedFactor -= cycle_time*3;
  }
  BackMButtonPressed = BackMPressedFactor.GetScaledValue();
}


void MATorso::GenerateBackRButtonPressed(int cycle_time)
{
  if (BackButtonR >= 13)
  {
    // Pressed "state" can be reached in 1 second
    BackRPressedFactor += cycle_time;
  } else {
    BackRPressedFactor -= cycle_time*3;
  }
  BackRButtonPressed = BackRPressedFactor.GetScaledValue();
}


void MATorso::GenerateBackStroked(int cycle_time)
{
  BackFTouched.PutValue(BackButtonF);
  BackMTouched.PutValue(BackButtonM);
  BackRTouched.PutValue(BackButtonR);

  if (unlikely(!BackFTouched.IsValid() || !BackMTouched.IsValid() || !BackRTouched.IsValid()))
    return;

  if (BackFTouched.IsStroked() || BackMTouched.IsStroked() || BackRTouched.IsStroked())
  {
    // The back stroked "state" can be reached in 1-2 seconds
    BackStrokedFactor += (int)((float)cycle_time*MCRand(10.0, 20.0) / 10.0);
  } else
  // Keep the current value if the BackR is not touched, but it was touched a moment ago.
  if (!BackFTouched.IsJustStroked() && !BackMTouched.IsJustStroked() && !BackRTouched.IsJustStroked())
  {
    BackStrokedFactor -= cycle_time;
  }
  BackStroked = BackStrokedFactor.GetScaledValue();
}


void MATorso::GenerateBackGrabbed(int cycle_time)
{
  int TouchedButtons = 0;

  TouchedButtons += BackButtonF > 0;
  TouchedButtons += BackButtonM > 0;
  TouchedButtons += BackButtonR > 0;

  if (TouchedButtons > 1 && BackButtonF+BackButtonM+BackButtonR > 50)
  {
    // The back grabbed "state" can be reached in 400 msec
    BackGrabbedFactor += cycle_time;
  } else {
    BackGrabbedFactor -= cycle_time;
  }
  BackGrabbed = BackGrabbedFactor.GetScaledValue();
}