00001 #include "Motion/MotionManager.h"
00002 #include "Motion/WalkMC.h"
00003 #include "Motion/MotionSequenceMC.h"
00004 #include "Motion/PostureEngine.h"
00005 #include "Shared/SharedObject.h"
00006 #include "Events/EventRouter.h"
00007 #include "Events/EventBase.h"
00008
00009 #include "DriveMeBehavior.h"
00010
00011 #include <iostream>
00012 #include <string>
00013 #include <stdio.h>
00014
00015
00016
00017
00018
00019
00020 DriveMeBehavior::DriveMeBehavior()
00021 : BehaviorBase("DriveMeBehavior"),
00022 walker_id(MotionManager::invalid_MC_ID),
00023 stand_id(MotionManager::invalid_MC_ID),
00024 stand(),
00025 last_dx(0), last_dy(0), last_da(0), last_time(5000)
00026 {
00027
00028 stand->setPlayTime(700);
00029 stand->setPose(PostureEngine("/ms/data/motion/stand.pos"));
00030 }
00031
00032 void DriveMeBehavior::DoStart()
00033 {
00034 BehaviorBase::DoStart();
00035
00036 walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00037
00038 stand_id = motman->addPersistentMotion(stand, MotionManager::kStdPriority+1);
00039
00040 erouter->addListener(this, EventBase::timerEGID);
00041
00042 erouter->addTimer(this, 0, 0, false);
00043 }
00044
00045 void DriveMeBehavior::DoStop()
00046 {
00047 BehaviorBase::DoStop();
00048
00049 erouter->removeTimer(this);
00050
00051 erouter->removeListener(this);
00052
00053 motman->removeMotion(walker_id);
00054 motman->removeMotion(stand_id);
00055 }
00056
00057
00058 void DriveMeBehavior::processEvent(const EventBase& event)
00059 {
00060 using namespace std;
00061
00062 WalkMC *walker;
00063 MotionSequenceMC<MotionSequence::SizeSmall> *standp;
00064
00065
00066 if(event.getGeneratorID() != EventBase::timerEGID) return;
00067
00068
00069 walker = (WalkMC*)motman->checkoutMotion(walker_id);
00070 walker->setTargetVelocity(0,0,0);
00071 motman->checkinMotion(walker_id);
00072
00073
00074 standp = (MotionSequenceMC<MotionSequence::SizeSmall>*)motman->checkoutMotion(stand_id);
00075 standp->play();
00076 motman->checkinMotion(stand_id);
00077
00078
00079 string instr;
00080 cout << "dx? [" << last_dx << "] >\t";
00081 cout.flush();
00082 getline(cin, instr);
00083 sscanf(instr.data(), "%lf", &last_dx);
00084
00085 cout << "dy? [" << last_dy << "] >\t";
00086 cout.flush();
00087 getline(cin, instr);
00088 sscanf(instr.data(), "%lf", &last_dy);
00089
00090 cout << "da? [" << last_da << "] >\t";
00091 cout.flush();
00092 getline(cin, instr);
00093 sscanf(instr.data(), "%lf", &last_da);
00094
00095 cout << "time? [" << last_time << "] >\t";
00096 cout.flush();
00097 getline(cin, instr);
00098 sscanf(instr.data(), "%u", &last_time);
00099
00100
00101 walker = (WalkMC*)motman->checkoutMotion(walker_id);
00102 walker->setTargetVelocity(last_dx, last_dy, last_da);
00103 erouter->addTimer(this, 0, last_time, false);
00104 motman->checkinMotion(walker_id);
00105 }
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117