Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

HeadPointControllerBehavior.cc

Go to the documentation of this file.

#include "HeadPointControllerBehavior.h"
#include "Behaviors/Controller.h"
#include "Motion/MMAccessor.h"

HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL;

void HeadPointControllerBehavior::runCommand(unsigned char *command) {
  // First, turn off the stop-if-no-heartbeat timer
  erouter->removeTimer(this);

  // Extract the command parameter
  float param;
  unsigned char *paramp = (unsigned char *) &param;

  paramp[0] = command[1];
  paramp[1] = command[2];
  paramp[2] = command[3];
  paramp[3] = command[4];

  // Find out what type of command this is
  switch(command[0]) {
  case CMD_tilt:
    t = param*outputRanges[HeadOffset+TiltOffset][MaxRange];
    break;
  case CMD_pan:
    p = param*outputRanges[HeadOffset+PanOffset][MaxRange];
    break;
  case CMD_roll:
    r = param*outputRanges[HeadOffset+RollOffset][MaxRange];
    break;
  default:
    cout << "MECHA: unknown command " << command[0] << endl;
  }

  // If the command was a new motion command, apply the
  // new motion parameters:
  switch(command[0]) {
  case CMD_tilt:
  case CMD_pan:
  case CMD_roll:
    {
      MMAccessor<HeadPointerMC> head(head_id);
      head->setJoints(t,p,r);
    }
  }

  // Reset the stop-if-no-heartbeat timer -- if we don't
  // hear from the mothership in three seconds, stop immediately.
  erouter->addTimer(this, 0, 3000, false);
}

void HeadPointControllerBehavior::DoStart() {
  // Behavior startup
  BehaviorBase::DoStart();
  // We listen to timers
  erouter->addListener(this, EventBase::timerEGID);
  // Enable head control
  head_id = motman->addMotion(SharedObject<HeadPointerMC>());
  // Turn on wireless
  theLastOne=theOne;
  theOne=this;
  wireless->setReceiver(cmdsock->sock, mechacmd_callback);
  wireless->listen(cmdsock->sock, config->main.headControl_port);
  // Open the WalkGUI on the desktop
  Controller::loadGUI("org.tekkotsu.mon.HeadPointGUI","HeadPointGUI",config->main.headControl_port);
}

void HeadPointControllerBehavior::DoStop() {
  // Close the GUI
  Controller::closeGUI("HeadPointGUI");
  // Turn off timers
  erouter->forgetListener(this);
  // Close socket; turn wireless off
  wireless->close(cmdsock);
  theOne=theLastOne;
  // Disable head pointer
  motman->removeMotion(head_id);
  // Total behavior stop
  BehaviorBase::DoStop();
}

// The command packet reassembly mechanism
int HeadPointControllerBehavior::mechacmd_callback(char *buf, int bytes) {
  static char cb_buf[5];
  static int cb_buf_filled;

  // If there's an incomplete command in the command buffer, fill
  // up as much of the command buffer as we can and then execute it
  // if possible
  if(cb_buf_filled) {
    while((cb_buf_filled < 5) && bytes) {
      cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
      --bytes;        // decrement remaining byte ct.
    }
    // did we fill it? if so, execute! and mark buffer empty.
    if(cb_buf_filled == 5) {
      if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
      cb_buf_filled = 0;
    }
  }

  // now execute all complete bytes in the incoming buffer
  while(bytes >= 5) {
    if(HeadPointControllerBehavior::theOne) HeadPointControllerBehavior::theOne->runCommand((unsigned char *) buf);
    bytes -= 5;
    buf += 5;
  }

  // finally, store all remaining bytes in the command buffer
  while(bytes) {
    cb_buf[cb_buf_filled++] = *buf++;
    --bytes;
  }

  return 0;
}

/*! @file
 * @brief Implements HeadPointControllerBehavior, listens to control commands coming in from the command port for remotely controlling the head
 * @author tss (Creator)
 * 
 * $Author: ejt $
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.1 $
 * $State: Rel $
 * $Date: 2003/10/10 00:45:01 $
 */

Tekkotsu v1.5

Generated Fri Oct 10 15:51:59 2003 by Doxygen 1.3.4