00001 #include "HeadPointControllerBehavior.h"
00002 #include "Behaviors/Controller.h"
00003 #include "Motion/MMAccessor.h"
00004 #include "Motion/HeadPointerMC.h"
00005 #include "Shared/ERS7Info.h"
00006 #include "Shared/ERS210Info.h"
00007
00008 HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL;
00009
00010 void HeadPointControllerBehavior::runCommand(unsigned char *command) {
00011
00012 float param;
00013 unsigned char *paramp = (unsigned char *) ¶m;
00014
00015 #if defined(BYTE_ORDER) && BYTE_ORDER==BIG_ENDIAN
00016 paramp[0] = command[4];
00017 paramp[1] = command[3];
00018 paramp[2] = command[2];
00019 paramp[3] = command[1];
00020 #else
00021 paramp[0] = command[1];
00022 paramp[1] = command[2];
00023 paramp[2] = command[3];
00024 paramp[3] = command[4];
00025 #endif
00026
00027
00028 #ifdef TGT_IS_AIBO
00029 switch(command[0]) {
00030 case CMD_tilt:
00031 t = fabs(param)*outputRanges[HeadOffset+TiltOffset][param>0?MaxRange:MinRange];
00032 break;
00033 case CMD_pan:
00034 p = fabs(param)*outputRanges[HeadOffset+PanOffset][param>0?MaxRange:MinRange];
00035 break;
00036 case CMD_roll:
00037 r = fabs(param)*outputRanges[HeadOffset+RollOffset][param>0?MaxRange:MinRange];
00038 break;
00039 default:
00040 std::cout << "MECHA: unknown command " << command[0] << std::endl;
00041 }
00042 #else
00043 switch(command[0]) {
00044 case CMD_tilt: {
00045 const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset];
00046 unsigned int i = capabilities.findOutputOffset(n);
00047 if(i!=-1U)
00048 t = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00049 } break;
00050 case CMD_pan: {
00051 const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset];
00052 unsigned int i = capabilities.findOutputOffset(n);
00053 if(i!=-1U)
00054 p = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00055 } break;
00056 case CMD_roll: {
00057 const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::NodOffset];
00058 unsigned int i = capabilities.findOutputOffset(n);
00059 if(i!=-1U)
00060 r = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00061 else {
00062 n = ERS210Info::outputNames[ERS210Info::HeadOffset+ERS210Info::RollOffset];
00063 i = capabilities.findOutputOffset(n);
00064 if(i!=-1U)
00065 r = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00066 }
00067 } break;
00068 default:
00069 std::cout << "MECHA: unknown command " << command[0] << std::endl;
00070 }
00071 #endif
00072
00073
00074
00075 switch(command[0]) {
00076 case CMD_tilt:
00077 case CMD_pan:
00078 case CMD_roll:
00079 {
00080 MMAccessor<HeadPointerMC> head(head_id);
00081 head->setJoints(t,p,r);
00082 }
00083 }
00084 }
00085
00086 void HeadPointControllerBehavior::DoStart() {
00087
00088 BehaviorBase::DoStart();
00089
00090 SharedObject<HeadPointerMC> head;
00091 head->setHold(false);
00092 head_id = motman->addPersistentMotion(head);
00093
00094 theLastOne=theOne;
00095 theOne=this;
00096 cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048);
00097 wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00098 wireless->setDaemon(cmdsock,true);
00099 wireless->listen(cmdsock->sock, config->main.headControl_port);
00100
00101 Controller::loadGUI("org.tekkotsu.mon.HeadPointGUI","HeadPointGUI",config->main.headControl_port);
00102 }
00103
00104 void HeadPointControllerBehavior::DoStop() {
00105
00106 Controller::closeGUI("HeadPointGUI");
00107
00108 erouter->removeListener(this);
00109
00110 wireless->setDaemon(cmdsock,false);
00111 wireless->close(cmdsock);
00112 theOne=theLastOne;
00113
00114 motman->removeMotion(head_id);
00115
00116 BehaviorBase::DoStop();
00117 }
00118
00119
00120 int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00121 static char cb_buf[5];
00122 static int cb_buf_filled;
00123
00124
00125
00126
00127 if(cb_buf_filled) {
00128 while((cb_buf_filled < 5) && bytes) {
00129 cb_buf[cb_buf_filled++] = *buf++;
00130 --bytes;
00131 }
00132
00133 if(cb_buf_filled == 5) {
00134 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00135 cb_buf_filled = 0;
00136 }
00137 }
00138
00139
00140 while(bytes >= 5) {
00141 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
00142 bytes -= 5;
00143 buf += 5;
00144 }
00145
00146
00147 while(bytes) {
00148 cb_buf[cb_buf_filled++] = *buf++;
00149 --bytes;
00150 }
00151
00152 return 0;
00153 }
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165