Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WalkControllerBehavior.cc

Go 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   // 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 #if defined(BYTE_ORDER) && BYTE_ORDER==BIG_ENDIAN
00018   paramp[0] = command[4];
00019   paramp[1] = command[3];
00020   paramp[2] = command[2];
00021   paramp[3] = command[1];
00022 #else
00023   paramp[0] = command[1];
00024   paramp[1] = command[2];
00025   paramp[2] = command[3];
00026   paramp[3] = command[4];
00027 #endif
00028 
00029   // Find out what type of command this is
00030   switch(command[0]) {
00031   case CMD_fwd:
00032     dx = param;
00033     break;
00034   case CMD_roto:
00035     da = param;
00036     break;
00037   case CMD_side:
00038     dy = param;
00039     break;
00040   case CMD_opt0:
00041     {
00042       /*      HeadPointerMC *head =
00043         (HeadPointerMC*)motman->checkoutMotion(head_id);
00044       head->setJoints(0,0,0);
00045       motman->checkinMotion(head_id);*/
00046       break;
00047     }
00048   case CMD_opt1:
00049   case CMD_opt2:
00050   case CMD_opt3:
00051   case CMD_opt4:
00052     cout << "MECHA: hey, reprogram this button!" << endl;
00053     break;
00054   case CMD_opt5:
00055     sndman->playFile("howl.wav");
00056     break;
00057   case CMD_opt6:
00058     sndman->playFile("yap.wav");
00059     break;
00060   case CMD_opt7:
00061     sndman->playFile("whimper.wav");
00062     break;
00063   case CMD_opt8:
00064     sndman->playFile("growl.wav");
00065     break;
00066   case CMD_opt9:
00067     sndman->playFile("barkmed.wav");
00068     break;
00069     // The options button commands.
00070   default:
00071     cout << "MECHA: unknown command " << command[0] << endl;
00072   }
00073 
00074   // If the command was a new motion command, apply the
00075   // new motion parameters:
00076   switch(command[0]) {
00077   case CMD_fwd:
00078   case CMD_roto:
00079   case CMD_side:
00080     {
00081       MMAccessor<WalkMC> walker(getWalkID());
00082       float tdx=dx*walker->getCP().max_vel[dx>0?WalkMC::CalibrationParam::forward:WalkMC::CalibrationParam::reverse];
00083       float tdy=dy*walker->getCP().max_vel[WalkMC::CalibrationParam::strafe];
00084       float tda=da*walker->getCP().max_vel[WalkMC::CalibrationParam::rotate];
00085       walker->setTargetVelocity(tdx,tdy,tda);
00086     }
00087   }
00088 
00089   // Reset the stop-if-no-heartbeat timer -- if we don't
00090   // hear from the mothership in three seconds, stop immediately.
00091   erouter->addTimer(this, 0, 3000, false);
00092 }
00093 
00094 void WalkControllerBehavior::DoStart() {
00095   // Behavior startup
00096   BehaviorBase::DoStart();
00097   // We listen to timers (but don't need to explicitly tell erouter -- addTimer implies this)
00098   //erouter->addListener(this, EventBase::timerEGID);
00099   // Enable walker (the MC_ID can be accessed through the shared_walker later)
00100   motman->addPersistentMotion(shared_walker);
00101   // Turn on wireless
00102   theLastOne=theOne;
00103   theOne=this;
00104   cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048);
00105   wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00106   wireless->setDaemon(cmdsock,true); 
00107   wireless->listen(cmdsock->sock, config->main.walkControl_port);
00108   // Open the WalkGUI on the desktop
00109   Controller::loadGUI("org.tekkotsu.mon.WalkGUI","WalkGUI",config->main.walkControl_port);
00110 }
00111 
00112 void WalkControllerBehavior::DoStop() {
00113   // Close the GUI
00114   Controller::closeGUI("WalkGUI");
00115   // Turn off timers
00116   erouter->removeListener(this);
00117   // Close socket; turn wireless off
00118   wireless->setDaemon(cmdsock,false); 
00119   wireless->close(cmdsock);
00120   theOne=theLastOne;
00121   // Disable walker
00122   motman->removeMotion(getWalkID());
00123   // Total behavior stop
00124   BehaviorBase::DoStop();
00125 }
00126 
00127 // The command packet reassembly mechanism
00128 int WalkControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00129   static char cb_buf[5];
00130   static int cb_buf_filled;
00131 
00132   // If there's an incomplete command in the command buffer, fill
00133   // up as much of the command buffer as we can and then execute it
00134   // if possible
00135   if(cb_buf_filled) {
00136     while((cb_buf_filled < 5) && bytes) {
00137       cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
00138       --bytes;        // decrement remaining byte ct.
00139     }
00140     // did we fill it? if so, execute! and mark buffer empty.
00141     if(cb_buf_filled == 5) {
00142       if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00143       cb_buf_filled = 0;
00144     }
00145   }
00146 
00147   // now execute all complete bytes in the incoming buffer
00148   while(bytes >= 5) {
00149     if(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char *) buf);
00150     bytes -= 5;
00151     buf += 5;
00152   }
00153 
00154   // finally, store all remaining bytes in the command buffer
00155   while(bytes) {
00156     cb_buf[cb_buf_filled++] = *buf++;
00157     --bytes;
00158   }
00159 
00160   return 0;
00161 }
00162 
00163 /*! @file
00164  * @brief Implements WalkControllerBehavior, listens to mecha control commands coming in from the command port for remotely controlling the walk
00165  * @author tss (Creator)
00166  * @author ejt (modifications)
00167  * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
00168  * 
00169  * $Author: ejt $
00170  * $Name: tekkotsu-4_0 $
00171  * $Revision: 1.13 $
00172  * $State: Exp $
00173  * $Date: 2007/05/21 20:51:20 $
00174  */
00175 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:56 2007 by Doxygen 1.5.4