Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

UPennWalkControllerBehavior.cc

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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4