Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
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 #include "Shared/ERS7Info.h"
00006 #include "Shared/ERS210Info.h"
00007 
00008 HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL;
00009 
00010 void HeadPointControllerBehavior::runCommand(unsigned char *command) {
00011   // Extract the command parameter
00012   float param;
00013   unsigned char *paramp = (unsigned char *) &param;
00014 
00015 #if defined(BYTE_ORDER) && BYTE_ORDER==BIG_ENDIAN
00016   paramp[0] = command[4];
00017   paramp[1] = command[3];
00018   paramp[2] = command[2];
00019   paramp[3] = command[1];
00020 #else
00021   paramp[0] = command[1];
00022   paramp[1] = command[2];
00023   paramp[2] = command[3];
00024   paramp[3] = command[4];
00025 #endif
00026   
00027   // Find out what type of command this is
00028 #ifdef TGT_IS_AIBO
00029   switch(command[0]) {
00030   case CMD_tilt:
00031     t = fabs(param)*outputRanges[HeadOffset+TiltOffset][param>0?MaxRange:MinRange];
00032     break;
00033   case CMD_pan:
00034     p = fabs(param)*outputRanges[HeadOffset+PanOffset][param>0?MaxRange:MinRange];
00035     break;
00036   case CMD_roll:
00037     r = fabs(param)*outputRanges[HeadOffset+RollOffset][param>0?MaxRange:MinRange];
00038     break;
00039   default:
00040     std::cout << "MECHA: unknown command " << command[0] << std::endl;
00041   }
00042 #else
00043   switch(command[0]) {
00044     case CMD_tilt: {
00045       const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset];
00046       unsigned int i = capabilities.findOutputOffset(n);
00047       if(i!=-1U)
00048         t = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00049     } break;
00050     case CMD_pan: {
00051       const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset];
00052       unsigned int i = capabilities.findOutputOffset(n);
00053       if(i!=-1U)
00054         p = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00055     } break;
00056     case CMD_roll: {
00057       const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::NodOffset];
00058       unsigned int i = capabilities.findOutputOffset(n);
00059       if(i!=-1U)
00060         r = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00061       else {
00062         n = ERS210Info::outputNames[ERS210Info::HeadOffset+ERS210Info::RollOffset];
00063         i = capabilities.findOutputOffset(n);
00064         if(i!=-1U)
00065           r = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange];
00066       }
00067     } break;
00068     default:
00069       std::cout << "MECHA: unknown command " << command[0] << std::endl;
00070   }
00071 #endif
00072 
00073   // If the command was a new motion command, apply the
00074   // new motion parameters:
00075   switch(command[0]) {
00076   case CMD_tilt:
00077   case CMD_pan:
00078   case CMD_roll:
00079     {
00080       MMAccessor<HeadPointerMC> head(head_id);
00081       head->setJoints(t,p,r);
00082     }
00083   }
00084 }
00085 
00086 void HeadPointControllerBehavior::DoStart() {
00087   // Behavior startup
00088   BehaviorBase::DoStart();
00089   // Enable head control
00090   SharedObject<HeadPointerMC> head;
00091   head->setHold(false);
00092   head_id = motman->addPersistentMotion(head);
00093   // Turn on wireless
00094   theLastOne=theOne;
00095   theOne=this;
00096   cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048);
00097   wireless->setReceiver(cmdsock->sock, mechacmd_callback);
00098   wireless->setDaemon(cmdsock,true);
00099   wireless->listen(cmdsock->sock, config->main.headControl_port);
00100   // Open the WalkGUI on the desktop
00101   Controller::loadGUI("org.tekkotsu.mon.HeadPointGUI","HeadPointGUI",config->main.headControl_port);
00102 }
00103 
00104 void HeadPointControllerBehavior::DoStop() {
00105   // Close the GUI
00106   Controller::closeGUI("HeadPointGUI");
00107   // Turn off timers
00108   erouter->removeListener(this);
00109   // Close socket; turn wireless off
00110   wireless->setDaemon(cmdsock,false);
00111   wireless->close(cmdsock);
00112   theOne=theLastOne;
00113   // Disable head pointer
00114   motman->removeMotion(head_id);
00115   // Total behavior stop
00116   BehaviorBase::DoStop();
00117 }
00118 
00119 // The command packet reassembly mechanism
00120 int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
00121   static char cb_buf[5];
00122   static int cb_buf_filled;
00123 
00124   // If there's an incomplete command in the command buffer, fill
00125   // up as much of the command buffer as we can and then execute it
00126   // if possible
00127   if(cb_buf_filled) {
00128     while((cb_buf_filled < 5) && bytes) {
00129       cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
00130       --bytes;        // decrement remaining byte ct.
00131     }
00132     // did we fill it? if so, execute! and mark buffer empty.
00133     if(cb_buf_filled == 5) {
00134       if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
00135       cb_buf_filled = 0;
00136     }
00137   }
00138 
00139   // now execute all complete bytes in the incoming buffer
00140   while(bytes >= 5) {
00141     if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
00142     bytes -= 5;
00143     buf += 5;
00144   }
00145 
00146   // finally, store all remaining bytes in the command buffer
00147   while(bytes) {
00148     cb_buf[cb_buf_filled++] = *buf++;
00149     --bytes;
00150   }
00151 
00152   return 0;
00153 }
00154 
00155 /*! @file
00156  * @brief Implements HeadPointControllerBehavior, listens to control commands coming in from the command port for remotely controlling the head
00157  * @author tss (Creator)
00158  * 
00159  * $Author: ejt $
00160  * $Name: tekkotsu-4_0 $
00161  * $Revision: 1.16 $
00162  * $State: Exp $
00163  * $Date: 2007/11/12 18:00:41 $
00164  */
00165 

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