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