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 void HeadPointControllerBehavior::DoStart() {
00050
00051 BehaviorBase::DoStart();
00052
00053 head_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00054
00055 theLastOne=theOne;
00056 theOne=this;
00057 cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00058 wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00059 wireless->setDaemon(cmdsock,true);
00060 wireless->listen(cmdsock->sock, config->main.headControl_port);
00061
00062 Controller::loadGUI("org.tekkotsu.mon.HeadPointGUI","HeadPointGUI",config->main.headControl_port);
00063 }
00064
00065 void HeadPointControllerBehavior::DoStop() {
00066
00067 Controller::closeGUI("HeadPointGUI");
00068
00069 erouter->removeListener(this);
00070
00071 wireless->setDaemon(cmdsock,false);
00072 wireless->close(cmdsock);
00073 theOne=theLastOne;
00074
00075 motman->removeMotion(head_id);
00076
00077 BehaviorBase::DoStop();
00078 }
00079
00080
00081 int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00082 static char cb_buf[5];
00083 static int cb_buf_filled;
00084
00085
00086
00087
00088 if(cb_buf_filled) {
00089 while((cb_buf_filled < 5) && bytes) {
00090 cb_buf[cb_buf_filled++] = *buf++;
00091 --bytes;
00092 }
00093
00094 if(cb_buf_filled == 5) {
00095 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00096 cb_buf_filled = 0;
00097 }
00098 }
00099
00100
00101 while(bytes >= 5) {
00102 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
00103 bytes -= 5;
00104 buf += 5;
00105 }
00106
00107
00108 while(bytes) {
00109 cb_buf[cb_buf_filled++] = *buf++;
00110 --bytes;
00111 }
00112
00113 return 0;
00114 }
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
|