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(walker_id);
00072 walker->setTargetVelocity(dx,dy,da);
00073 }
00074 }
00075
00076
00077
00078 erouter->addTimer(this, 0, 3000, false);
00079 }
00080
00081 void WalkControllerBehavior::DoStart() {
00082
00083 BehaviorBase::DoStart();
00084
00085 erouter->addListener(this, EventBase::timerEGID);
00086
00087 walker_id = motman->addMotion(SharedObject<WalkMC>());
00088
00089 wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00090 wireless->listen(cmdsock->sock, config->main.walkControl_port);
00091
00092 Controller::loadGUI("WalkGUI","WalkGUI",config->main.walkControl_port);
00093 }
00094
00095 void WalkControllerBehavior::DoStop() {
00096
00097 Controller::closeGUI("WalkGUI");
00098
00099 erouter->forgetListener(this);
00100
00101 wireless->close(cmdsock);
00102
00103 motman->removeMotion(walker_id);
00104
00105 BehaviorBase::DoStop();
00106 }
00107
00108
00109 int WalkControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00110 static char cb_buf[5];
00111 static int cb_buf_filled;
00112
00113
00114
00115
00116 if(cb_buf_filled) {
00117 while((cb_buf_filled < 5) && bytes) {
00118 cb_buf[cb_buf_filled++] = *buf++;
00119 --bytes;
00120 }
00121
00122 if(cb_buf_filled == 5) {
00123 if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00124 cb_buf_filled = 0;
00125 }
00126 }
00127
00128
00129 while(bytes >= 5) {
00130 if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char *) buf);
00131 bytes -= 5;
00132 buf += 5;
00133 }
00134
00135
00136 while(bytes) {
00137 cb_buf[cb_buf_filled++] = *buf++;
00138 --bytes;
00139 }
00140
00141 return 0;
00142 }
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154