Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

WalkControllerBehavior.cc

Go to the documentation of this file.

#include "WalkControllerBehavior.h"
#include "Behaviors/Controller.h"

WalkControllerBehavior* WalkControllerBehavior::theOne = NULL;

void WalkControllerBehavior::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_fwd:
    dx = param;
    break;
  case CMD_roto:
    da = param;
    break;
  case CMD_side:
    dy = param;
    break;
  case CMD_opt0:
    {
      /*      HeadPointerMC *head =
        (HeadPointerMC*)motman->checkoutMotion(head_id);
      head->setJoints(0,0,0);
      motman->checkinMotion(head_id);*/
      break;
    }
  case CMD_opt1:
  case CMD_opt2:
  case CMD_opt3:
  case CMD_opt4:
    cout << "MECHA: hey, reprogram this button!" << endl;
    break;
  case CMD_opt5:
    sndman->PlayFile("howl.wav");
    break;
  case CMD_opt6:
    sndman->PlayFile("yap.wav");
    break;
  case CMD_opt7:
    sndman->PlayFile("whimper.wav");
    break;
  case CMD_opt8:
    sndman->PlayFile("growl.wav");
    break;
  case CMD_opt9:
    sndman->PlayFile("barkmed.wav");
    break;
    // The options button commands.
  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_fwd:
  case CMD_roto:
  case CMD_side:
    {
      MMAccessor<WalkMC> walker(getWalkID());
      walker->setTargetVelocity(dx,dy,da);
    }
  }

  // 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 WalkControllerBehavior::DoStart() {
  // Behavior startup
  BehaviorBase::DoStart();
  // We listen to timers
  erouter->addListener(this, EventBase::timerEGID);
  // Enable walker (the MC_ID can be accessed through the shared_walker later)
  motman->addMotion(shared_walker);
  // Turn on wireless
  theLastOne=theOne;
  theOne=this;
  wireless->setReceiver(cmdsock->sock, mechacmd_callback);
  wireless->listen(cmdsock->sock, config->main.walkControl_port);
  // Open the WalkGUI on the desktop
  Controller::loadGUI("org.tekkotsu.mon.WalkGUI","WalkGUI",config->main.walkControl_port);
}

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

// The command packet reassembly mechanism
int WalkControllerBehavior::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(WalkControllerBehavior::theOne) WalkControllerBehavior::theOne->runCommand((unsigned char*) cb_buf);
      cb_buf_filled = 0;
    }
  }

  // now execute all complete bytes in the incoming buffer
  while(bytes >= 5) {
    if(WalkControllerBehavior::theOne) WalkControllerBehavior::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 WalkControllerBehavior, listens to mecha control commands coming in from the command port for remotely controlling the walk
 * @author tss (Creator)
 * @author ejt (modifications)
 * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
 * 
 * $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:52:00 2003 by Doxygen 1.3.4