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

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