ArmController.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_ArmController_h_
#define INCLUDED_ArmController_h_

#include <iostream>
#include "Wireless/Wireless.h"
#include "Behaviors/BehaviorBase.h"
#include "Motion/MotionManager.h"
#include "Events/EventRouter.h"
#include "Events/EventBase.h"
#include "Shared/Config.h"
#include "Motion/PlanarThreeLinkArm.h"
#include "Motion/HandEyeGripper.h"
#include "Motion/PIDMC.h"
#include "IPC/SharedObject.h"
#include "Events/EventRouter.h"
#include "Motion/KinematicJoint.h"

//! Listens to control commands coming in from the command port for remotely controlling the arm.
/*! The communication protocol is a very simple binary format, shared
 *  with WalkControllerBehavior.  Each command is sent as a 5-byte
 *  group.  The first byte is a command selector, and the following 4
 *  bytes are a floating point argument:
 *
 *  - <@c char: command indicator>
 *  - <@c float: value>
 *  
 *  The valid values for command indicator are given by #CMD_shoulder,
 *  #CMD_elbow, or #CMD_wrist ('s', 'e', or 'w' respectively).
 */  
class ArmController : public BehaviorBase {
  
public: 
  //! Points to the one ArmController object that the input
  //! command stream is talking to. A kludge. Dunno how you're gonna
  //! make sure you're not using this uninitialized.
  static ArmController * theOne;
  static int mechacmd_callback(char *buf, int bytes); //!< called by wireless when there's new data
  
protected:
  MotionManager::MC_ID arm_id;   //!< the ArmMC to use
  MotionManager::MC_ID pidMCID;
  HandEyeGripper GripperControl;
  KinematicJoint* gripperFrameKJ;
  KinematicJoint* KJjoints[NumArmJoints];
  float horScale, verScale; 
  
  enum DisplayMode_t {
    pitchAndYaw = 0,
    yawOnly,
    pitchOnly
  } displayMode;
  
  //!@name Command Bytes        
  static const char CMD_unrelax  = 'u';
  static const char CMD_relax  = 'v';
  static const char CMD_gripper  = 'w';
  static const char CMD_point  = 'x';
  static const char CMD_speed  = 'y';
  static const char CMD_connect  = 'z';
  
  float theta;
  float verZ;
  fmat::Column<3> horToBase;
  fmat::Column<3> verToBase;
  float speed;
  unsigned int OriJoint;
  int linksToDisplay;
  
  //Joint configuration info
  char armConfig[NumArmJoints];
  float qOffset[NumArmJoints];
  float yawCoords[NumArmJoints][2]; //Yaw Joints
  float pitchCoords[NumArmJoints][2]; //Pitch Joints
  int numYawJoints;
  int numPitchJoints;
  
  //! The last HPCB object that was theOne, so we can restore it
  //! to prominence when we die. This is a nice gesture, but it doesn't
  //! really make sense since we're all using the same port. But just
  //! in case something changes and we don't do that, this mechanism
  //! is in place.
  ArmController *theLastOne;
  
  //! The input command stream socket
  Socket *cmdsock;
  
  //! Executes a command. Called by mechacmd_callback.
  void runCommand(unsigned char *command);
  void doEvent();
  
  void cmdConnect();
  //int chooseAngles(PlanarThreeLinkArm::Solutions sol);
  //void setArmLengths(int jointNo, char jointType);
  void cmdPointPicker(float param, float param2, float param3, int cmdno);
  void cmdGripper(float param, int cmdno);
  void cmdRelax();
  void cmdUnrelax();
  void setJoint(unsigned int joint, float param);
  
  ArmController(const ArmController&); //!< don't call
  ArmController operator=(const ArmController&); //!< don't call
  
public:
  //! constructor
  ArmController() : BehaviorBase("ArmController"),
  arm_id(MotionManager::invalid_MC_ID), pidMCID(MotionManager::invalid_MC_ID),
  GripperControl(), gripperFrameKJ(NULL), KJjoints(), 
  horScale(0), verScale(0), displayMode(), theta(0), verZ(0),
  horToBase(), verToBase(), speed(0.4f), OriJoint(), 
  linksToDisplay(0), yawCoords(),  pitchCoords(), numYawJoints(0), numPitchJoints(0),
  theLastOne(theOne),  /*Set the default to the robot last joint before the gripper*/
  cmdsock(NULL)
  {
    for(unsigned int i =0; i < (NumArmJoints+1); i++)
      armConfig[i] = '\0';
    gripperFrameKJ = kine->getKinematicJoint(GripperFrameOffset)->cloneBranch();
  }
  
  //! destructor
  virtual ~ArmController() {
    theOne = theLastOne;
    delete gripperFrameKJ;
    gripperFrameKJ=NULL;
  }
  
  //! Setup the scale and the joints quantity to display
  virtual void doStart();
  
  virtual void doStop();
  
  void computeCoords();
  void sendCoords();  
  
  static std::string getClassDescription() {
    char tmp[20];
    sprintf(tmp,"%d",*config->main.armControl_port);
    return std::string("Listens to arm control commands coming in from port ")+tmp;
  }
  virtual std::string getDescription() const { return getClassDescription(); }
};

/*! @file
 * @brief Describes ArmController, listens to control commands coming in from the command port for remotely controlling the head
 * @author tss (Creator)
 */

#endif