00001 #include "HeadPointControllerBehavior.h"
00002 #include "Behaviors/Controller.h"
00003 #include "Motion/MMAccessor.h"
00004
00005 HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL;
00006
00007 void HeadPointControllerBehavior::runCommand(unsigned char *command) {
00008
00009 erouter->removeTimer(this);
00010
00011
00012 float param;
00013 unsigned char *paramp = (unsigned char *) ¶m;
00014
00015 paramp[0] = command[1];
00016 paramp[1] = command[2];
00017 paramp[2] = command[3];
00018 paramp[3] = command[4];
00019
00020
00021 switch(command[0]) {
00022 case CMD_tilt:
00023 t = fabs(param)*outputRanges[HeadOffset+TiltOffset][param>0?MaxRange:MinRange];
00024 break;
00025 case CMD_pan:
00026 p = fabs(param)*outputRanges[HeadOffset+PanOffset][param>0?MaxRange:MinRange];
00027 break;
00028 case CMD_roll:
00029 r = fabs(param)*outputRanges[HeadOffset+RollOffset][param>0?MaxRange:MinRange];
00030 break;
00031 default:
00032 cout << "MECHA: unknown command " << command[0] << endl;
00033 }
00034
00035
00036
00037 switch(command[0]) {
00038 case CMD_tilt:
00039 case CMD_pan:
00040 case CMD_roll:
00041 {
00042 MMAccessor<HeadPointerMC> head(head_id);
00043 head->setJoints(t,p,r);
00044 }
00045 }
00046
00047
00048
00049 erouter->addTimer(this, 0, 3000, false);
00050 }
00051
00052 void HeadPointControllerBehavior::DoStart() {
00053
00054 BehaviorBase::DoStart();
00055
00056 erouter->addListener(this, EventBase::timerEGID);
00057
00058 head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00059
00060 theLastOne=theOne;
00061 theOne=this;
00062 cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00063 wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00064 wireless->setDaemon(cmdsock,true);
00065 wireless->listen(cmdsock->sock, config->main.headControl_port);
00066
00067 Controller::loadGUI("org.tekkotsu.mon.HeadPointGUI","HeadPointGUI",config->main.headControl_port);
00068 }
00069
00070 void HeadPointControllerBehavior::DoStop() {
00071
00072 Controller::closeGUI("HeadPointGUI");
00073
00074 erouter->forgetListener(this);
00075
00076 wireless->setDaemon(cmdsock,false);
00077 wireless->close(cmdsock);
00078 theOne=theLastOne;
00079
00080 motman->removeMotion(head_id);
00081
00082 BehaviorBase::DoStop();
00083 }
00084
00085
00086 int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00087 static char cb_buf[5];
00088 static int cb_buf_filled;
00089
00090
00091
00092
00093 if(cb_buf_filled) {
00094 while((cb_buf_filled < 5) && bytes) {
00095 cb_buf[cb_buf_filled++] = *buf++;
00096 --bytes;
00097 }
00098
00099 if(cb_buf_filled == 5) {
00100 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00101 cb_buf_filled = 0;
00102 }
00103 }
00104
00105
00106 while(bytes >= 5) {
00107 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
00108 bytes -= 5;
00109 buf += 5;
00110 }
00111
00112
00113 while(bytes) {
00114 cb_buf[cb_buf_filled++] = *buf++;
00115 --bytes;
00116 }
00117
00118 return 0;
00119 }
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131