00001
00002 #ifndef INCLUDED_Aibo3DControllerBehavior_h_
00003 #define INCLUDED_Aibo3DControllerBehavior_h_
00004
00005 #include <iostream>
00006 #include "Wireless/Wireless.h"
00007 #include "Behaviors/BehaviorBase.h"
00008 #include "Motion/MotionManager.h"
00009 #include "Motion/RemoteControllerMC.h"
00010 #include "Events/EventRouter.h"
00011 #include "Events/EventBase.h"
00012 #include "Shared/RobotInfo.h"
00013 #include "Behaviors/Controller.h"
00014 #include "Shared/WorldState.h"
00015
00016
00017 int aibo3dcontrollercmd_callback(char *buf, int bytes);
00018 class Aibo3DControllerBehavior;
00019
00020
00021 Aibo3DControllerBehavior *aibo3dControllerBehavior = NULL;
00022
00023
00024
00025 class Aibo3DControllerBehavior : public BehaviorBase {
00026 protected:
00027 MotionManager::MC_ID rcontrol_id;
00028
00029
00030 Socket *cmdsock;
00031
00032 float val[NumPIDJoints];
00033 char *fbuf;
00034 unsigned int pos;
00035
00036 private:
00037 Aibo3DControllerBehavior(const Aibo3DControllerBehavior&);
00038 Aibo3DControllerBehavior operator=(const Aibo3DControllerBehavior&);
00039
00040 public:
00041
00042 Aibo3DControllerBehavior() :
00043 BehaviorBase(),
00044 rcontrol_id(MotionManager::invalid_MC_ID),
00045 cmdsock(NULL),
00046 fbuf((char*)val), pos(0)
00047 { aibo3dControllerBehavior = this; }
00048
00049 virtual ~Aibo3DControllerBehavior() { aibo3dControllerBehavior = NULL; }
00050
00051
00052 int registerData(char *buf, int bytes) {
00053 int read=0;
00054 while (read<bytes) {
00055 fbuf[pos]=buf[read];
00056 pos++;
00057 read++;
00058 if (pos==NumPIDJoints*sizeof(float)) {
00059 updateRC();
00060 pos=0;
00061 }
00062 }
00063 return read;
00064 }
00065
00066
00067 void updateRC() {
00068 RemoteControllerMC *rcontrol = (RemoteControllerMC*)motman->checkoutMotion(rcontrol_id);
00069 for (unsigned int i=0; i<NumPIDJoints; i++)
00070 rcontrol->cmds[i]=val[i];
00071 rcontrol->setDirty();
00072 motman->checkinMotion(rcontrol_id);
00073 }
00074
00075 virtual void DoStart() {
00076
00077 BehaviorBase::DoStart();
00078 for(unsigned int i=0; i<NumPIDJoints; i++)
00079 val[i]=state->outputs[i];
00080
00081 rcontrol_id = motman->addPersistentMotion(SharedObject<RemoteControllerMC>());
00082 updateRC();
00083
00084 cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00085 wireless->setReceiver(cmdsock->sock, aibo3dcontrollercmd_callback);
00086 wireless->setDaemon(cmdsock,true);
00087 wireless->listen(cmdsock->sock, config->main.aibo3d_port);
00088
00089
00090
00091
00092
00093
00094 Controller::loadGUI(getGUIType(),getGUIType(),getPort());
00095 }
00096
00097 virtual void DoStop() {
00098 Controller::closeGUI(getGUIType());
00099
00100 wireless->setDaemon(cmdsock,false);
00101 wireless->close(cmdsock);
00102
00103 motman->removeMotion(rcontrol_id);
00104
00105 BehaviorBase::DoStop();
00106 }
00107
00108
00109 virtual std::string getGUIType() const { return "org.tekkotsu.aibo3d.Aibo3DPick"; }
00110
00111 virtual unsigned int getPort() const { return config->main.aibo3d_port; }
00112
00113 virtual std::string getName() const { return "Aibo 3D"; }
00114 static std::string getClassDescription() {
00115 char tmp[20];
00116 sprintf(tmp,"%d",config->main.aibo3d_port);
00117 return std::string("Listens to aibo3d control commands coming in from port ")+tmp;
00118 }
00119 };
00120
00121
00122 int aibo3dcontrollercmd_callback(char *buf, int bytes) {
00123 if (aibo3dControllerBehavior!=NULL)
00124 return aibo3dControllerBehavior->registerData(buf, bytes);
00125 return 0;
00126 }
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139 #endif