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