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