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 = param*outputRanges[HeadOffset+TiltOffset][MaxRange];
00024 break;
00025 case CMD_pan:
00026 p = param*outputRanges[HeadOffset+PanOffset][MaxRange];
00027 break;
00028 case CMD_roll:
00029 r = param*outputRanges[HeadOffset+RollOffset][MaxRange];
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 wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00061 wireless->listen(cmdsock->sock, config->main.headControl_port);
00062
00063 Controller::loadGUI("HeadPointGUI","HeadPointGUI",config->main.headControl_port);
00064 }
00065
00066 void HeadPointControllerBehavior::DoStop() {
00067
00068 Controller::closeGUI("HeadPointGUI");
00069
00070 erouter->forgetListener(this);
00071
00072 wireless->close(cmdsock);
00073
00074 motman->removeMotion(head_id);
00075
00076 BehaviorBase::DoStop();
00077 }
00078
00079
00080 int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00081 static char cb_buf[5];
00082 static int cb_buf_filled;
00083
00084
00085
00086
00087 if(cb_buf_filled) {
00088 while((cb_buf_filled < 5) && bytes) {
00089 cb_buf[cb_buf_filled++] = *buf++;
00090 --bytes;
00091 }
00092
00093 if(cb_buf_filled == 5) {
00094 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00095 cb_buf_filled = 0;
00096 }
00097 }
00098
00099
00100 while(bytes >= 5) {
00101 if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
00102 bytes -= 5;
00103 buf += 5;
00104 }
00105
00106
00107 while(bytes) {
00108 cb_buf[cb_buf_filled++] = *buf++;
00109 --bytes;
00110 }
00111
00112 return 0;
00113 }
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125