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