ArP2Arm.h

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#ifndef ARP2ARM_H
#define ARP2ARM_H

#include "ArRobot.h"
#include "ariaTypedefs.h"
#include "ArSerialConnection.h"
#include "ariaOSDef.h"

// P2 Arm classes: ArP2Arm for control and ArP2ArmJoints for P2 Arm joint data

class P2ArmJoint
{
public:

  P2ArmJoint();
  virtual ~P2ArmJoint();

  ArTypes::UByte myPos;
  ArTypes::UByte myVel;
  ArTypes::UByte myHome;
  ArTypes::UByte myMin;
  ArTypes::UByte myCenter;
  ArTypes::UByte myMax;
  ArTypes::UByte myTicksPer90;
};


class ArP2Arm
{
public:

  typedef enum {
    SUCCESS, 
    ALREADY_INITED, 
    NOT_INITED, 
    ROBOT_NOT_SETUP, 
    NO_ARM_FOUND, 
    COMM_FAILED, 
    COULD_NOT_OPEN_PORT, 
    COULD_NOT_SET_UP_PORT, 
    ALREADY_CONNECTED, 
    NOT_CONNECTED, 
    INVALID_JOINT, 
    INVALID_POSITION 
    } State;

  typedef enum {
    StatusPacket, 
    InfoPacket 
  } PacketType;

  typedef enum
  {
    StatusOff=0, 
    StatusSingle=1, 
    StatusContinuous=2 
  } StatusType;

  static const int ArmJoint1;
  static const int ArmJoint2;
  static const int ArmJoint3;
  static const int ArmJoint4;
  static const int ArmJoint5;
  static const int ArmJoint6;
  static const int ArmGood;
  static const int ArmInited;
  static const int ArmPower;
  static const int ArmHoming;
  static int NumJoints;

  ArP2Arm();

  virtual ~ArP2Arm();

  void setRobot(ArRobot *robot) {myRobot=robot;}

  virtual State init();

  virtual State uninit();

  virtual State powerOn(bool doWait=true);

  virtual State powerOff();

  virtual State requestInfo();

  virtual State requestStatus(StatusType status);

  virtual State requestInit();

  virtual State checkArm(bool waitForResponse=true);

  virtual State home(int joint=-1);

  virtual State park();

  virtual State moveTo(int joint, float pos, unsigned char vel=0);

  virtual State moveToTicks(int joint, unsigned char pos);

  virtual State moveStep(int joint, float pos, unsigned char vel=0);

  virtual State moveStepTicks(int joint, signed char pos);

  virtual State moveVel(int joint, int vel);

  virtual State stop();

  virtual State setAutoParkTimer(int waitSecs);

  virtual State setGripperParkTimer(int waitSecs);

  virtual void setStoppedCB(ArFunctor *func) {myStoppedCB=func;}

  virtual void setPacketCB(ArFunctor1<PacketType> *func)
    {myPacketCB=func;}

  virtual std::string getArmVersion() {return(myVersion);}

  virtual float getJointPos(int joint);

  virtual unsigned char getJointPosTicks(int joint);

  virtual bool getMoving(int joint=-1);

  virtual bool isPowered();

  virtual bool isGood();

  virtual int getStatus() {return(myStatus);}

  virtual ArTime getLastStatusTime() {return(myLastStatusTime);}

  virtual ArRobot * getRobot() {return(myRobot);}

  virtual P2ArmJoint * getJoint(int joint);

  virtual bool convertDegToTicks(int joint, float pos,
                                          unsigned char *ticks);

  virtual bool convertTicksToDeg(int joint, unsigned char pos,
                                          float *degrees);


protected:

  // AROS/P2OS parameters
  static const unsigned int ARMpac;
  static const unsigned int ARMINFOpac;
  static const unsigned char ComArmInfo;
  static const unsigned char ComArmStats;
  static const unsigned char ComArmInit;
  static const unsigned char ComArmCheckArm;
  static const unsigned char ComArmPower;
  static const unsigned char ComArmHome;
  static const unsigned char ComArmPark;
  static const unsigned char ComArmPos;
  static const unsigned char ComArmSpeed;
  static const unsigned char ComArmStop;
  static const unsigned char ComArmAutoPark;
  static const unsigned char ComArmGripperPark;

  bool comArmInfo();
  bool comArmStats(StatusType stats=StatusSingle);
  bool comArmInit();
  bool comArmCheckArm();
  bool comArmPower(bool on);
  bool comArmHome(unsigned char joint=0xff);
  bool comArmPark();
  bool comArmPos(unsigned char joint, unsigned char pos);
  bool comArmSpeed(unsigned char joint, unsigned char speed);
  bool comArmStop(unsigned char joint=0xff);
  bool comArmAutoPark(int waitSecs);
  bool comArmGripperPark(int waitSecs);

  bool armPacketHandler(ArRobotPacket *packet);

  bool myInited;
  ArRobot *myRobot;
  //  ArmP2Model myModel;
  ArTime myLastStatusTime;
  ArTime myLastInfoTime;
  std::string myVersion;
  StatusType myStatusRequest;
  ArTypes::UByte2 myLastStatus;
  ArTypes::UByte2 myStatus;
  ArSerialConnection myCon;
  ArRetFunctorC<State, ArP2Arm> myAriaUninitCB;
  ArRetFunctor1C<bool, ArP2Arm, ArRobotPacket*> myArmPacketHandler;
  ArFunctor1<PacketType> *myPacketCB;
  ArFunctor *myStoppedCB;

  // We have 6 joints. Including the gripper. It's here so that we can
  // store its position even though its not really a joint.
  P2ArmJoint myJoints[6];
};

#endif // _ARP2ARM_H

---