Homepage Demos Overview Downloads Tutorials Reference
Credits

WalkControllerBehavior.cc

Go to the documentation of this file.
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   // First, turn off the stop-if-no-heartbeat timer
00009   erouter->removeTimer(this);
00010 
00011   // Extract the command parameter
00012   float param;
00013   unsigned char *paramp = (unsigned char *) &param;
00014 
00015   paramp[0] = command[1];
00016   paramp[1] = command[2];
00017   paramp[2] = command[3];
00018   paramp[3] = command[4];
00019 
00020   // Find out what type of command this is
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       /*      HeadPointerMC *head =
00034         (HeadPointerMC*)motman->checkoutMotion(head_id);
00035       head->setJoints(0,0,0);
00036       motman->checkinMotion(head_id);*/
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     // The options button commands.
00061   default:
00062     cout << "MECHA: unknown command " << command[0] << endl;
00063   }
00064 
00065   // If the command was a new motion command, apply the
00066   // new motion parameters:
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   // Reset the stop-if-no-heartbeat timer -- if we don't
00081   // hear from the mothership in three seconds, stop immediately.
00082   erouter->addTimer(this, 0, 3000, false);
00083 }
00084 
00085 void WalkControllerBehavior::DoStart() {
00086   // Behavior startup
00087   BehaviorBase::DoStart();
00088   // We listen to timers
00089   erouter->addListener(this, EventBase::timerEGID);
00090   // Enable walker (the MC_ID can be accessed through the shared_walker later)
00091   motman->addPersistentMotion(shared_walker);
00092   // Turn on wireless
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   // Open the WalkGUI on the desktop
00100   Controller::loadGUI("org.tekkotsu.mon.WalkGUI","WalkGUI",config->main.walkControl_port);
00101 }
00102 
00103 void WalkControllerBehavior::DoStop() {
00104   // Close the GUI
00105   Controller::closeGUI("WalkGUI");
00106   // Turn off timers
00107   erouter->removeListener(this);
00108   // Close socket; turn wireless off
00109   wireless->setDaemon(cmdsock,false); 
00110   wireless->close(cmdsock);
00111   theOne=theLastOne;
00112   // Disable walker
00113   motman->removeMotion(getWalkID());
00114   // Total behavior stop
00115   BehaviorBase::DoStop();
00116 }
00117 
00118 // The command packet reassembly mechanism
00119 int WalkControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00120   static char cb_buf[5];
00121   static int cb_buf_filled;
00122 
00123   // If there's an incomplete command in the command buffer, fill
00124   // up as much of the command buffer as we can and then execute it
00125   // if possible
00126   if(cb_buf_filled) {
00127     while((cb_buf_filled < 5) && bytes) {
00128       cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
00129       --bytes;        // decrement remaining byte ct.
00130     }
00131     // did we fill it? if so, execute! and mark buffer empty.
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   // now execute all complete bytes in the incoming buffer
00139   while(bytes >= 5) {
00140     if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char *) buf);
00141     bytes -= 5;
00142     buf += 5;
00143   }
00144 
00145   // finally, store all remaining bytes in the command buffer
00146   while(bytes) {
00147     cb_buf[cb_buf_filled++] = *buf++;
00148     --bytes;
00149   }
00150 
00151   return 0;
00152 }
00153 
00154 /*! @file
00155  * @brief Implements WalkControllerBehavior, listens to mecha control commands coming in from the command port for remotely controlling the walk
00156  * @author tss (Creator)
00157  * @author ejt (modifications)
00158  * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
00159  * 
00160  * $Author: ejt $
00161  * $Name: tekkotsu-2_2_2 $
00162  * $Revision: 1.7 $
00163  * $State: Exp $
00164  * $Date: 2004/11/04 03:01:32 $
00165  */
00166 

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:16 2005 by Doxygen 1.4.0