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 
00004 WalkControllerBehavior* WalkControllerBehavior::theOne = NULL;
00005 
00006 void WalkControllerBehavior::runCommand(unsigned char *command) {
00007   // First, turn off the stop-if-no-heartbeat timer
00008   erouter->removeTimer(this);
00009 
00010   // Extract the command parameter
00011   float param;
00012   unsigned char *paramp = (unsigned char *) &param;
00013 
00014   paramp[0] = command[1];
00015   paramp[1] = command[2];
00016   paramp[2] = command[3];
00017   paramp[3] = command[4];
00018 
00019   // Find out what type of command this is
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       /*      HeadPointerMC *head =
00033         (HeadPointerMC*)motman->checkoutMotion(head_id);
00034       head->setJoints(0,0,0);
00035       motman->checkinMotion(head_id);*/
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     // The options button commands.
00060   default:
00061     cout << "MECHA: unknown command " << command[0] << endl;
00062   }
00063 
00064   // If the command was a new motion command, apply the
00065   // new motion parameters:
00066   switch(command[0]) {
00067   case CMD_fwd:
00068   case CMD_roto:
00069   case CMD_side:
00070     {
00071       MMAccessor<WalkMC> walker(getWalkID());
00072       float tdx=dx*walker->getCP().max_vel[dx>0?WalkMC::CalibrationParam::forward:WalkMC::CalibrationParam::reverse];
00073       float tdy=dy*walker->getCP().max_vel[WalkMC::CalibrationParam::strafe];
00074       float tda=da*walker->getCP().max_vel[WalkMC::CalibrationParam::rotate];
00075       walker->setTargetVelocity(tdx,tdy,tda);
00076     }
00077   }
00078 
00079   // Reset the stop-if-no-heartbeat timer -- if we don't
00080   // hear from the mothership in three seconds, stop immediately.
00081   erouter->addTimer(this, 0, 3000, false);
00082 }
00083 
00084 void WalkControllerBehavior::DoStart() {
00085   // Behavior startup
00086   BehaviorBase::DoStart();
00087   // We listen to timers
00088   erouter->addListener(this, EventBase::timerEGID);
00089   // Enable walker (the MC_ID can be accessed through the shared_walker later)
00090   motman->addMotion(shared_walker);
00091   // Turn on wireless
00092   theLastOne=theOne;
00093   theOne=this;
00094   cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00095   wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00096   wireless->setDaemon(cmdsock,true); 
00097   wireless->listen(cmdsock->sock, config->main.walkControl_port);
00098   // Open the WalkGUI on the desktop
00099   Controller::loadGUI("org.tekkotsu.mon.WalkGUI","WalkGUI",config->main.walkControl_port);
00100 }
00101 
00102 void WalkControllerBehavior::DoStop() {
00103   // Close the GUI
00104   Controller::closeGUI("WalkGUI");
00105   // Turn off timers
00106   erouter->forgetListener(this);
00107   // Close socket; turn wireless off
00108   wireless->setDaemon(cmdsock,false); 
00109   wireless->close(cmdsock);
00110   theOne=theLastOne;
00111   // Disable walker
00112   motman->removeMotion(getWalkID());
00113   // Total behavior stop
00114   BehaviorBase::DoStop();
00115 }
00116 
00117 // The command packet reassembly mechanism
00118 int WalkControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00119   static char cb_buf[5];
00120   static int cb_buf_filled;
00121 
00122   // If there's an incomplete command in the command buffer, fill
00123   // up as much of the command buffer as we can and then execute it
00124   // if possible
00125   if(cb_buf_filled) {
00126     while((cb_buf_filled < 5) && bytes) {
00127       cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
00128       --bytes;        // decrement remaining byte ct.
00129     }
00130     // did we fill it? if so, execute! and mark buffer empty.
00131     if(cb_buf_filled == 5) {
00132       if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00133       cb_buf_filled = 0;
00134     }
00135   }
00136 
00137   // now execute all complete bytes in the incoming buffer
00138   while(bytes >= 5) {
00139     if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char *) buf);
00140     bytes -= 5;
00141     buf += 5;
00142   }
00143 
00144   // finally, store all remaining bytes in the command buffer
00145   while(bytes) {
00146     cb_buf[cb_buf_filled++] = *buf++;
00147     --bytes;
00148   }
00149 
00150   return 0;
00151 }
00152 
00153 /*! @file
00154  * @brief Implements WalkControllerBehavior, listens to mecha control commands coming in from the command port for remotely controlling the walk
00155  * @author tss (Creator)
00156  * @author ejt (modifications)
00157  * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
00158  * 
00159  * $Author: ejt $
00160  * $Name: tekkotsu-2_1 $
00161  * $Revision: 1.3 $
00162  * $State: Exp $
00163  * $Date: 2004/01/19 19:43:51 $
00164  */
00165 

Tekkotsu v2.1
Generated Tue Mar 16 23:19:16 2004 by Doxygen 1.3.5