Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

StartupBehavior.cc

Go to the documentation of this file.

#include "StartupBehavior.h"

#include "Behaviors/Controller.h"
#include "Behaviors/Controls/BatteryCheckControl.h"
#include "Behaviors/Controls/ControlBase.h"
#include "Behaviors/Controls/HelpControl.h"
#include "Behaviors/Controls/RebootControl.h"
#include "Behaviors/Controls/ShutdownControl.h"

#include "Motion/MotionCommand.h"
#include "Motion/PostureMC.h"
#include "Motion/EmergencyStopMC.h"
#include "Motion/PIDMC.h"
#include "Motion/MotionSequenceMC.h"
#include "Motion/MMAccessor.h"

#include "Vision/Vision.h"

#include "SoundPlay/SoundManager.h"

#include "Shared/ERS210Info.h"

StartupBehavior theStartup; //!< used to initialize the global ::startupBehavior, used by MMCombo
BehaviorBase& startupBehavior=theStartup; //!< used by MMCombo as the init behavior

StartupBehavior::StartupBehavior()
  : BehaviorBase(), spawned(),setup(),
    stop_id(MotionManager::invalid_MC_ID),
    pid_id(MotionManager::invalid_MC_ID)
{
  AddReference(); // this is a global, so there's a global reference
}

StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;}

void StartupBehavior::DoStart() {
  BehaviorBase::DoStart();
  
  //This will "fade" in the PIDs so the joints don't jerk to full power, also looks cooler
  pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+1,false);
  //also, pause before we start fading in, PIDs take effect right away, before the emergencystop is picked up
  erouter->addTimer(this,0,4*FrameTime*NumFrames,true);

  //This is the default emergency stop
  const SharedObject<EmergencyStopMC> stop;
  stop->LoadFile("/ms/data/motion/liedown.pos"); //This *should* be replaced by the current position, but just in case, better than setting everything to 0's
  stop->setStopped(true,false); //if you want to start off paused
  //sndman->StopPlay(); //so it doesn't play the halt sound the first time (but the false in previous line does that now)
  stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority);
  
  //This displays the current battery conditions
  BatteryCheckControl batchk;
  batchk.activate(MotionManager::invalid_MC_ID,NULL);
  //  const SharedObject<LedMC> led; //! @todo LedMC's don't support autopruning yet, it should for uses like this, or could the one in Controller
  //  batchk.activate(motman->addMotion(led,true));
  batchk.deactivate();

  //This is for the menu system
  Controller * controller=new Controller;
  controller->DoStart();
  controller->setEStopID(stop_id);
  controller->setRoot(SetupMenus());
  wireless->setReceiver(sout, Controller::console_callback);
  spawned.push_back(controller);
  
  sndman->PlayFile("roar.wav");

  //This will close the mouth so it doesn't look stupid
  //Now done by setting the emergency stop directly in processEvent, but left as demo code:
  /*  const SharedObject<PostureMC> closemouth;
      closemouth->setJointCmd(MouthOffset,outputRanges[MouthOffset][MaxRange],1);
      motman->addMotion(closemouth,MotionCommand::kEmergencyPriority+2,true);
  */
}

void StartupBehavior::DoStop() {
  for(std::vector<BehaviorBase*>::iterator it=spawned.begin(); it!=spawned.end(); it++)
    (*it)->DoStop();
  motman->removeMotion(stop_id);
  BehaviorBase::DoStop();
}

/*!Uses a few timer events at the beginning to fade in the PID values, and closes the mouth too*/
void StartupBehavior::processEvent(const EventBase&) {
  static unsigned int start_time=-1U;
  const unsigned int tot_time=2047; //if this is set to 2048, i sometimes get funny errors: [oid:80000034,prio:1] AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.
  if(start_time==-1U) { //first time
    start_time=get_time();
    MMAccessor<EmergencyStopMC>(stop_id)->takeSnapshot(); //take new snapshot with hopefully valid data
  } else {
    float power=(get_time()-start_time)/(float)tot_time;
    if(power>1)
      power=1;
    { MMAccessor<PIDMC>(pid_id)->setAllPowerLevel(power); }
    if(state->robotDesign & WorldState::ERS210Mask)
      { MMAccessor<EmergencyStopMC>(stop_id)->setOutputCmd(ERS210Info::MouthOffset,outputRanges[ERS210Info::MouthOffset][MaxRange]); }
  }
  if((get_time()-start_time)>=tot_time) {
    erouter->removeTimer(this);
    motman->removeMotion(pid_id);
    pid_id=MotionManager::invalid_MC_ID;
  }
}

/*class WalkToPinkBallFactory : public Factory<WalkToTargetMachine> {
  public:
  static WalkToTargetMachine* construct() { return new WalkToTargetMachine(VisionEventNS::PinkBallSID); }
  };
*/

ControlBase*
StartupBehavior::SetupMenus() {
  std::cout << "CONTROLLER-INIT..." << std::flush;
  ControlBase* root=new ControlBase("Root Control");
  setup.push(root);

  // additional controls will be submenus of root:
  {
    SetupModeSwitch();
    SetupBackgroundBehaviors();
    SetupTekkotsuMon();
    SetupStatusReports();
    SetupFileAccess();
    SetupWalkEdit();
    addItem(new ControlBase("Shutdown?"));
    startSubMenu();
    { 
      addItem(new ShutdownControl());
      addItem(new RebootControl());
    }
    endSubMenu();
    addItem(new HelpControl(root));
  }
  
  if(endSubMenu()!=root)
    cout << "\n*** WARNING *** menu setup stack corrupted" << endl;
  cout << "DONE" << endl;
  return root;
}

void
StartupBehavior::startSubMenu() {
  setup.push(setup.top()->getSlots().back());
}

void
StartupBehavior::addItem(ControlBase * control) {
  setup.top()->pushSlot(control);
}

ControlBase*
StartupBehavior::endSubMenu() {
  ControlBase * tmp=setup.top();
  setup.pop();
  return tmp;
}

Tekkotsu v1.5

Generated Fri Oct 10 15:52:00 2003 by Doxygen 1.3.4