Homepage Demos Overview Downloads Tutorials Reference
Credits

HeadPointControllerBehavior.cc

Go to the documentation of this file.
00001 #include "HeadPointControllerBehavior.h"
00002 #include "Behaviors/Controller.h"
00003 #include "Motion/MMAccessor.h"
00004 #include "Motion/HeadPointerMC.h"
00005 
00006 HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL;
00007 
00008 void HeadPointControllerBehavior::runCommand(unsigned char *command) {
00009   // First, turn off the stop-if-no-heartbeat timer
00010   erouter->removeTimer(this);
00011 
00012   // Extract the command parameter
00013   float param;
00014   unsigned char *paramp = (unsigned char *) &param;
00015 
00016   paramp[0] = command[1];
00017   paramp[1] = command[2];
00018   paramp[2] = command[3];
00019   paramp[3] = command[4];
00020 
00021   // Find out what type of command this is
00022   switch(command[0]) {
00023   case CMD_tilt:
00024     t = fabs(param)*outputRanges[HeadOffset+TiltOffset][param>0?MaxRange:MinRange];
00025     break;
00026   case CMD_pan:
00027     p = fabs(param)*outputRanges[HeadOffset+PanOffset][param>0?MaxRange:MinRange];
00028     break;
00029   case CMD_roll:
00030     r = fabs(param)*outputRanges[HeadOffset+RollOffset][param>0?MaxRange:MinRange];
00031     break;
00032   default:
00033     cout << "MECHA: unknown command " << command[0] << endl;
00034   }
00035 
00036   // If the command was a new motion command, apply the
00037   // new motion parameters:
00038   switch(command[0]) {
00039   case CMD_tilt:
00040   case CMD_pan:
00041   case CMD_roll:
00042     {
00043       MMAccessor<HeadPointerMC> head(head_id);
00044       head->setJoints(t,p,r);
00045     }
00046   }
00047 
00048   // Reset the stop-if-no-heartbeat timer -- if we don't
00049   // hear from the mothership in three seconds, stop immediately.
00050   erouter->addTimer(this, 0, 3000, false);
00051 }
00052 
00053 void HeadPointControllerBehavior::DoStart() {
00054   // Behavior startup
00055   BehaviorBase::DoStart();
00056   // We listen to timers
00057   erouter->addListener(this, EventBase::timerEGID);
00058   // Enable head control
00059   head_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00060   // Turn on wireless
00061   theLastOne=theOne;
00062   theOne=this;
00063   cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00064   wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00065   wireless->setDaemon(cmdsock,true);
00066   wireless->listen(cmdsock->sock, config->main.headControl_port);
00067   // Open the WalkGUI on the desktop
00068   Controller::loadGUI("org.tekkotsu.mon.HeadPointGUI","HeadPointGUI",config->main.headControl_port);
00069 }
00070 
00071 void HeadPointControllerBehavior::DoStop() {
00072   // Close the GUI
00073   Controller::closeGUI("HeadPointGUI");
00074   // Turn off timers
00075   erouter->removeListener(this);
00076   // Close socket; turn wireless off
00077   wireless->setDaemon(cmdsock,false);
00078   wireless->close(cmdsock);
00079   theOne=theLastOne;
00080   // Disable head pointer
00081   motman->removeMotion(head_id);
00082   // Total behavior stop
00083   BehaviorBase::DoStop();
00084 }
00085 
00086 // The command packet reassembly mechanism
00087 int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00088   static char cb_buf[5];
00089   static int cb_buf_filled;
00090 
00091   // If there's an incomplete command in the command buffer, fill
00092   // up as much of the command buffer as we can and then execute it
00093   // if possible
00094   if(cb_buf_filled) {
00095     while((cb_buf_filled < 5) && bytes) {
00096       cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
00097       --bytes;        // decrement remaining byte ct.
00098     }
00099     // did we fill it? if so, execute! and mark buffer empty.
00100     if(cb_buf_filled == 5) {
00101       if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00102       cb_buf_filled = 0;
00103     }
00104   }
00105 
00106   // now execute all complete bytes in the incoming buffer
00107   while(bytes >= 5) {
00108     if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
00109     bytes -= 5;
00110     buf += 5;
00111   }
00112 
00113   // finally, store all remaining bytes in the command buffer
00114   while(bytes) {
00115     cb_buf[cb_buf_filled++] = *buf++;
00116     --bytes;
00117   }
00118 
00119   return 0;
00120 }
00121 
00122 /*! @file
00123  * @brief Implements HeadPointControllerBehavior, listens to control commands coming in from the command port for remotely controlling the head
00124  * @author tss (Creator)
00125  * 
00126  * $Author: ejt $
00127  * $Name: tekkotsu-2_2_2 $
00128  * $Revision: 1.7 $
00129  * $State: Exp $
00130  * $Date: 2004/10/17 01:16:10 $
00131  */
00132 

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