erratic.h

// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-

#ifndef _ERRATICDEVICE_H
#define _ERRATICDEVICE_H

#ifndef ERRATIC_VERSION
#define ERRATIC_VERSION "1.0b"
#endif

#ifndef ERRATIC_DATE
#define ERRATIC_DATE "2006-05-07"
#endif

#include <pthread.h>
#include <sys/time.h>
#include <queue>

#include <libplayercore/playercore.h>
#include <replace/replace.h>

#include "packet.h"
#include "robot_params.h"

//#include <stdint.h>

#define CPU_VOLTAGE 3.5

// angular constants, angular units are 4096 / rev
#define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
#define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
#define RTOA(x) ((short)((x) * 2048.0) / M_PI)

// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)

// This merely sets a delay policy in the initial connection
#define ROBOT_CYCLETIME 20000

/* Erratic constants */

#define VIDERE_NOMINAL_VOLTAGE 12.0


// Commands for the robot
typedef enum command {
        pulse =                     0,
        open_controller =           1,
        close_controller =          2,
        enable_motors =             4,
        set_max_trans_acc =         5,
        set_max_position_velocity = 6,
        reset_origo =               7,
        trans_vel =                 11, // mm/s
        rot_pos   =                 12, // deg
        rot_dpos  =                 13, // deg
        configuration =             18,
        rot_vel =                   21, // deg/s
        set_max_rot_acc =           23,
        set_sonar =                 28,
        stop =                      29,
        wheel_vel =                 32, // mm/s
        set_analog =                71,
        save_config =               72,
        set_pwm_freq =              73,
        set_pwm_max_on =            74,
        servo_pos      =            75,
        set_pid_trans_p =           80,
        set_pid_trans_v =           81,
        set_pid_trans_i =           82,
        set_pid_rot_p =             83,
        set_pid_rot_v =             84,
        set_pid_rot_i =             85

} command_e;

// Argument types used in robot commands
typedef enum argtype {
        argint =  0x3B,
        argnint = 0x1B,
        argstr =  0x2B
} argtype_e;

// Types of replies from the robot
typedef enum reply {
        debug =   0x15,
        config =  0x20,
        stopped = 0x32,
        moving =  0x33,
        motor =   0x80,
        encoder = 0x90,
        ain =     0x9a,
        sonar =   0x9b
} reply_e;


#define DEFAULT_VIDERE_PORT "/dev/erratic"

typedef struct player_erratic_data
{
  player_position2d_data_t position;
  player_power_data_t power;
  player_aio_data_t aio;
  player_ir_data ir;
  player_sonar_data sonar;
} __attribute__ ((packed)) player_erratic_data_t;

// this is here because we need the above typedef's before including it.
#include "motorpacket.h"

extern bool debug_mode;

class ErraticMotorPacket;

class Erratic : public Driver
{
private:
  int mcount;
  player_erratic_data_t erratic_data;

  player_devaddr_t position_id;
  player_devaddr_t power_id;
  player_devaddr_t aio_id;
  player_devaddr_t ir_id;
  player_devaddr_t sonar_id;
  player_devaddr_t ptz_id, ptz2_id;

  int position_subscriptions;
  int aio_ir_subscriptions;
  int sonar_subscriptions;
  int ptz_subscriptions;
  int ptz2_subscriptions;

  //ErraticMotorPacket* sippacket;
  ErraticMotorPacket *motor_packet;
  pthread_mutex_t motor_packet_mutex;

  int Connect();
  int Disconnect();

  void ResetRawPositions();
  void ToggleMotorPower(unsigned char val);

  void ToggleAIn(unsigned char val);
  void ToggleSonar(unsigned char val);

  int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
  int HandleCommand(player_msghdr * hdr, void * data);
  void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
  void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
  void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id);

  void PublishAllData();
  void PublishPosition2D();
  void PublishPower();
  void PublishAIn();
  void PublishIR();
  void PublishSonar();

  float IRRangeFromVoltage(float voltage);
  float IRFloorRange(float value);

  void StartThreads();
  void StopThreads();

  void Send(ErraticPacket *packet);
  void SendThread();
  static void *SendThreadDummy(void *driver);
  void ReceiveThread();
  static void *ReceiveThreadDummy(void *driver);

  int read_fd, write_fd;
  const char* psos_serial_port;

  player_position2d_cmd_vel_t last_position_cmd;
  player_position2d_cmd_car_t last_car_cmd;

  std::queue<ErraticPacket *> send_queue;
  pthread_mutex_t send_queue_mutex;
  pthread_cond_t send_queue_cond;

  pthread_t send_thread;
  pthread_t receive_thread;

  // Parameters

  bool direct_wheel_vel_control;

  bool print_all_packets;
  bool print_status_summary;

  bool save_settings_in_robot;

  int param_idx;  // index in the RobotParams table for this robot

  // Max motor speeds (mm/sec,deg/sec)
  int motor_max_speed;
  int motor_max_turnspeed;

  // Customized control settings for the robot
  int16_t pid_trans_p, pid_trans_v, pid_trans_i;
  int16_t pid_rot_p, pid_rot_v, pid_rot_i;

  // This is a fairly low-level setting that is exposed
  uint16_t motor_pwm_frequency, motor_pwm_max_on;

  // Bound the command velocities
  bool use_vel_band;

  // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
  short motor_max_trans_accel, motor_max_trans_decel;
  short motor_max_rot_accel, motor_max_rot_decel;

public:

  Erratic(ConfigFile* cf, int section);

  virtual int Subscribe(player_devaddr_t id);
  virtual int Unsubscribe(player_devaddr_t id);

  /* the main thread */
  virtual void Main();

  virtual int Setup();
  virtual int Shutdown();

  // MessageHandler
  virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
};


#endif