00001 #include "WalkControllerBehavior.h"
00002 #include "Behaviors/Controller.h"
00003
00004 WalkControllerBehavior* WalkControllerBehavior::theOne = NULL;
00005
00006 void WalkControllerBehavior::runCommand(unsigned char *command) {
00007
00008 erouter->removeTimer(this);
00009
00010
00011 float param;
00012 unsigned char *paramp = (unsigned char *) ¶m;
00013
00014 paramp[0] = command[1];
00015 paramp[1] = command[2];
00016 paramp[2] = command[3];
00017 paramp[3] = command[4];
00018
00019
00020 switch(command[0]) {
00021 case CMD_fwd:
00022 dx = param;
00023 break;
00024 case CMD_roto:
00025 da = param;
00026 break;
00027 case CMD_side:
00028 dy = param;
00029 break;
00030 case CMD_opt0:
00031 {
00032
00033
00034
00035
00036 break;
00037 }
00038 case CMD_opt1:
00039 case CMD_opt2:
00040 case CMD_opt3:
00041 case CMD_opt4:
00042 cout << "MECHA: hey, reprogram this button!" << endl;
00043 break;
00044 case CMD_opt5:
00045 sndman->PlayFile("howl.wav");
00046 break;
00047 case CMD_opt6:
00048 sndman->PlayFile("yap.wav");
00049 break;
00050 case CMD_opt7:
00051 sndman->PlayFile("whimper.wav");
00052 break;
00053 case CMD_opt8:
00054 sndman->PlayFile("growl.wav");
00055 break;
00056 case CMD_opt9:
00057 sndman->PlayFile("barkmed.wav");
00058 break;
00059
00060 default:
00061 cout << "MECHA: unknown command " << command[0] << endl;
00062 }
00063
00064
00065
00066 switch(command[0]) {
00067 case CMD_fwd:
00068 case CMD_roto:
00069 case CMD_side:
00070 {
00071 MMAccessor<WalkMC> walker(getWalkID());
00072 float tdx=dx*walker->getCP().max_vel[dx>0?WalkMC::CalibrationParam::forward:WalkMC::CalibrationParam::reverse];
00073 float tdy=dy*walker->getCP().max_vel[WalkMC::CalibrationParam::strafe];
00074 float tda=da*walker->getCP().max_vel[WalkMC::CalibrationParam::rotate];
00075 walker->setTargetVelocity(tdx,tdy,tda);
00076 }
00077 }
00078
00079
00080
00081 erouter->addTimer(this, 0, 3000, false);
00082 }
00083
00084 void WalkControllerBehavior::DoStart() {
00085
00086 BehaviorBase::DoStart();
00087
00088 erouter->addListener(this, EventBase::timerEGID);
00089
00090 motman->addMotion(shared_walker);
00091
00092 theLastOne=theOne;
00093 theOne=this;
00094 cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00095 wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00096 wireless->setDaemon(cmdsock,true);
00097 wireless->listen(cmdsock->sock, config->main.walkControl_port);
00098
00099 Controller::loadGUI("org.tekkotsu.mon.WalkGUI","WalkGUI",config->main.walkControl_port);
00100 }
00101
00102 void WalkControllerBehavior::DoStop() {
00103
00104 Controller::closeGUI("WalkGUI");
00105
00106 erouter->forgetListener(this);
00107
00108 wireless->setDaemon(cmdsock,false);
00109 wireless->close(cmdsock);
00110 theOne=theLastOne;
00111
00112 motman->removeMotion(getWalkID());
00113
00114 BehaviorBase::DoStop();
00115 }
00116
00117
00118 int WalkControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00119 static char cb_buf[5];
00120 static int cb_buf_filled;
00121
00122
00123
00124
00125 if(cb_buf_filled) {
00126 while((cb_buf_filled < 5) && bytes) {
00127 cb_buf[cb_buf_filled++] = *buf++;
00128 --bytes;
00129 }
00130
00131 if(cb_buf_filled == 5) {
00132 if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00133 cb_buf_filled = 0;
00134 }
00135 }
00136
00137
00138 while(bytes >= 5) {
00139 if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char *) buf);
00140 bytes -= 5;
00141 buf += 5;
00142 }
00143
00144
00145 while(bytes) {
00146 cb_buf[cb_buf_filled++] = *buf++;
00147 --bytes;
00148 }
00149
00150 return 0;
00151 }
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165