runCtrl.h

Go to the documentation of this file.

#ifndef RUN_CTRL_H
#define RUN_CTRL_H

#include "runInterface.h"
#include "ticksCtrlInterface.h"
#include "connectionDevice.h"
#include "structTables.h"
#include "nodeAccess.h"
#include "timeUtils.h"
#include <list>


class RunCtrl : public ObjCoordinateCtrl, public RunInterface,
                public TicksCtrlInterface {
  friend class PacketHandleHostTest;
  friend class tRunCtrl_Simulator;
  friend class MonitorTask;

protected:
  enum {
    SimulatorPort = 49754,
  };

  enum Default {
    Baudrate = 115200,          
    StraightVel = 300,          
    StraightAcc = 600,          
    RotateVel = 0x10000 >> 1,   
    RotateAcc = 0x10000 >> 1,   
    FollowRadius = 500,         
  };

private:
  ConnectionDevice* con;
  nodeInfo_t* node;
  long unique_id;
  bool crd_auto;
  nodeInfo_t nodeResource;

protected:
  VXV::Position local_offset;   
private:
  std::string error_message;

  typedef struct {
    int straight_ref_vel;
    int straight_ref_acc;
    long rotate_ref_vel;
    long rotate_ref_acc;
    int follow_r;
  } runParams_t;

  enum { SEND_COMMAND_SIZE = 256 };
  typedef struct {
    VXV::Position position;
    const CoordinateCtrl* crd;
    int send_command_size;
    char send_command[SEND_COMMAND_SIZE];
  } runCommand_t;

  typedef struct {
    runParams_t params;
    runCommand_t command;
  } runState_t;

  std::list<runState_t> state_stack;

  unsigned long pre_module_msec;
  unsigned long total_msec;
  ticksInfo_t ticksInfoObj;
  ticksInfo_t& getTicksInfo(void) {
    return ticksInfoObj;
  }
  void beginTimeAdjust(void);
  void endTimeAdjust(void);
  unsigned long getModuleRawTicks(void);

  void initRunParams(void);
  void printHelp(void);
  void printVersion(void);
  bool parseArgs(int* ret_value, int argc, char *argv[]);
  int checkVersion(void);
  void crdNortify(int state, const CoordinateCtrl* crd);
  const VXV::Position getRunCmdPos(const CoordinateCtrl* crd,
                                   const VXV::Position& position);
  void sendUpdatedPosition(const CoordinateCtrl* crd,
                           const VXV::Position& position);
  unsigned long updateTicksDiff(unsigned long ticks);
  RunCtrl(const RunCtrl& rhs);

#include "commandCtrl.h"

protected:
  runCtrl_t* tbl;               
  runCtrl_t tblResource;

  int to_div16(const VXV::Direction& t);

  virtual int raw_connect(ConnectionDevice* conObj,
                          const char* device, long baudrate);

  virtual unsigned long getHostTicks(void);

  virtual void initTicksInfo(void);

public:
  enum {
    LineOver = +1, LineNotOver = -1,
    DefaultTimeout = -1,
  };
  CoordinateCtrl& FS;           
  RunCtrl(void);
  virtual ~RunCtrl(void);

  const char* what(void);

  virtual int connect(int argc, char *argv[]);

  virtual int connect(const char* device, long baudrate = Baudrate);

  virtual int connect(void);

  int connectSocket(const char* host, long port = SimulatorPort);

  virtual void disconnect(void);

  virtual bool isConnected(void);

  void push_runState(void);
  void pop_runState(void);


  void set_sendRetryTimes(int times);

  void set_recvTimeout(int timeout = DefaultTimeout);

  void set_watchDogTimer(unsigned long msec);

  virtual VXV::Position3D getLocalPosition(void);
  VXV::Position getRunPosition(const CoordinateCtrl* crd = VXV::GL);
  void adjustRunPosition(const VXV::Position& position,
                         const CoordinateCtrl* crd = VXV::GL);
  void coordinateUpdateDetect(bool on);

  virtual void stop(void);
  virtual void followLine(const VXV::Position& position,
                          const CoordinateCtrl* crd = VXV::GL);
  virtual void followCircle(const VXV::Grid& center, int radius,
                            const CoordinateCtrl* crd = VXV::GL);
  void followCircleOnTangent(const VXV::Position& position, int radius,
                             const CoordinateCtrl* crd = VXV::GL);
  virtual void stopToLine(const VXV::Position& position,
                          const CoordinateCtrl* crd = VXV::GL);
  virtual void rotateToDirection(const VXV::Direction& direction,
                                 const CoordinateCtrl* crd = VXV::GL);

  virtual void rotateAngle(const VXV::Direction& direction);
  virtual void spin(const VXV::Direction& velocity);
  virtual void lastMoveCommand(const CoordinateCtrl* crd = VXV::GL);

  virtual bool isStable(void);
  int getLengthToGrid(const VXV::Grid& grid,
                      const CoordinateCtrl* crd = VXV::GL);
  int getLengthToBody(const VXV::Grid& grid,
                      const CoordinateCtrl* crd = VXV::GL);
  int getLengthToLine(const VXV::Position& line,
                      const CoordinateCtrl* crd = VXV::GL);
  VXV::Direction getAngleToDirection(const VXV::Direction& t,
                                     const CoordinateCtrl* crd = VXV::GL);
  int getStraightVelDiff(int mm_vel = 0);
  VXV::Direction getRotateVelDiff(const VXV::Direction& t = VXV::Direction());

  void setStraightRefVel(int mm_sec);
  void setStraightRefAcc(int mm_acc);
  void setRotateRefVel(const VXV::Direction& rotate_vel);
  void setRotateRefAcc(const VXV::Direction& rotate_acc);
  void setCurveRadius(int mm);
  int getStraightVel(void);
  VXV::Direction getRotateVel(void);
  int getCurveRadius(void);

  virtual void servoCtrl(bool on);

  void setMotorPwm(int id, unsigned char duty);

  void setMotorPwm(int id, unsigned char duty[], int num);

  void setMotorMode(int id, unsigned char mode);

  void setMotorMode(int id, unsigned char mode[], int num);

  void getEncoderVel(int id, int *cnt);

  void getEncoderVel(int id, int cnt[], int num);

  void getEncoderValue(int id, unsigned short *cnt);

  void getEncoderValue(int id, unsigned short cnt[], int num);

  void setWheelVel(int id, int mm_vel);

  void setWheelVel(int id, int mm_vel[], int num);

  VXV::Position getNoResetRunPosition(void);
  int getBodyRotateCount(void);
  void getEncoderRotateCount(int id, int count[], int num = 1);

};


class RunCtrl_Exception : public std::exception {
  std::string error_message;

public:

  RunCtrl_Exception(const char* message) throw () : error_message(message){}
  virtual ~RunCtrl_Exception(void) throw () {}

  virtual const char* what(void) const throw() {
    return error_message.c_str();
  }
};

#endif /* !RUN_CTRL_H */