erratic.h
00001 // -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
00002 
00026 #ifndef _ERRATICDEVICE_H
00027 #define _ERRATICDEVICE_H
00028 
00029 #ifndef ERRATIC_VERSION
00030 #define ERRATIC_VERSION "1.0b"
00031 #endif
00032 
00033 #ifndef ERRATIC_DATE
00034 #define ERRATIC_DATE "2006-05-07"
00035 #endif
00036 
00037 #include <pthread.h>
00038 #include <sys/time.h>
00039 #include <queue>
00040 
00041 #include <libplayercore/playercore.h>
00042 #include <replace/replace.h>
00043 
00044 #include "packet.h"
00045 #include "robot_params.h"
00046 
00047 //#include <stdint.h>
00048 
00049 #define CPU_VOLTAGE 3.5
00050 
00051 // angular constants, angular units are 4096 / rev
00052 #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
00053 #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
00054 #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
00055 
00056 // Default max speeds
00057 #define MOTOR_DEF_MAX_SPEED 0.5
00058 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00059 
00060 // This merely sets a delay policy in the initial connection
00061 #define ROBOT_CYCLETIME 20000
00062 
00063 /* Erratic constants */
00064 
00065 #define VIDERE_NOMINAL_VOLTAGE 12.0
00066 
00067 
00068 // Commands for the robot
00069 typedef enum command {
00070         pulse =                     0,
00071         open_controller =           1,
00072         close_controller =          2,
00073         enable_motors =             4,
00074         set_max_trans_acc =         5,
00075         set_max_position_velocity = 6,
00076         reset_origo =               7,
00077         trans_vel =                 11, // mm/s
00078         rot_pos   =                 12, // deg
00079         rot_dpos  =                 13, // deg
00080         configuration =             18,
00081         rot_vel =                   21, // deg/s
00082         set_max_rot_acc =           23,
00083         set_sonar =                 28,
00084         stop =                      29,
00085         wheel_vel =                 32, // mm/s
00086         set_analog =                71,
00087         save_config =               72,
00088         set_pwm_freq =              73,
00089         set_pwm_max_on =            74,
00090         servo_pos      =            75,
00091         set_pid_trans_p =           80,
00092         set_pid_trans_v =           81,
00093         set_pid_trans_i =           82,
00094         set_pid_rot_p =             83,
00095         set_pid_rot_v =             84,
00096         set_pid_rot_i =             85
00097 
00098 } command_e;
00099 
00100 // Argument types used in robot commands
00101 typedef enum argtype {
00102         argint =  0x3B,
00103         argnint = 0x1B,
00104         argstr =  0x2B
00105 } argtype_e;
00106 
00107 // Types of replies from the robot
00108 typedef enum reply {
00109         debug =   0x15,
00110         config =  0x20,
00111         stopped = 0x32,
00112         moving =  0x33,
00113         motor =   0x80,
00114         encoder = 0x90,
00115         ain =     0x9a,
00116         sonar =   0x9b
00117 } reply_e;
00118 
00119 
00120 #define DEFAULT_VIDERE_PORT "/dev/erratic"
00121 
00122 typedef struct player_erratic_data
00123 {
00124   player_position2d_data_t position;
00125   player_power_data_t power;
00126   player_aio_data_t aio;
00127   player_ir_data ir;
00128   player_sonar_data sonar;
00129 } __attribute__ ((packed)) player_erratic_data_t;
00130 
00131 // this is here because we need the above typedef's before including it.
00132 #include "motorpacket.h"
00133 
00134 extern bool debug_mode;
00135 
00136 class ErraticMotorPacket;
00137 
00138 class Erratic : public Driver
00139 {
00140 private:
00141   int mcount;
00142   player_erratic_data_t erratic_data;
00143 
00144   player_devaddr_t position_id;
00145   player_devaddr_t power_id;
00146   player_devaddr_t aio_id;
00147   player_devaddr_t ir_id;
00148   player_devaddr_t sonar_id;
00149   player_devaddr_t ptz_id, ptz2_id;
00150 
00151   int position_subscriptions;
00152   int aio_ir_subscriptions;
00153   int sonar_subscriptions;
00154   int ptz_subscriptions;
00155   int ptz2_subscriptions;
00156 
00157   //ErraticMotorPacket* sippacket;
00158   ErraticMotorPacket *motor_packet;
00159   pthread_mutex_t motor_packet_mutex;
00160 
00161   int Connect();
00162   int Disconnect();
00163 
00164   void ResetRawPositions();
00165   void ToggleMotorPower(unsigned char val);
00166 
00167   void ToggleAIn(unsigned char val);
00168   void ToggleSonar(unsigned char val);
00169 
00170   int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
00171   int HandleCommand(player_msghdr * hdr, void * data);
00172   void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
00173   void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
00174   void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id);
00175 
00176   void PublishAllData();
00177   void PublishPosition2D();
00178   void PublishPower();
00179   void PublishAIn();
00180   void PublishIR();
00181   void PublishSonar();
00182 
00183   float IRRangeFromVoltage(float voltage);
00184   float IRFloorRange(float value);
00185 
00186   void StartThreads();
00187   void StopThreads();
00188 
00189   void Send(ErraticPacket *packet);
00190   void SendThread();
00191   static void *SendThreadDummy(void *driver);
00192   void ReceiveThread();
00193   static void *ReceiveThreadDummy(void *driver);
00194 
00195   int read_fd, write_fd;
00196   const char* psos_serial_port;
00197 
00198   player_position2d_cmd_vel_t last_position_cmd;
00199   player_position2d_cmd_car_t last_car_cmd;
00200 
00201   std::queue<ErraticPacket *> send_queue;
00202   pthread_mutex_t send_queue_mutex;
00203   pthread_cond_t send_queue_cond;
00204 
00205   pthread_t send_thread;
00206   pthread_t receive_thread;
00207 
00208   // Parameters
00209 
00210   bool direct_wheel_vel_control;
00211 
00212   bool print_all_packets;
00213   bool print_status_summary;
00214 
00215   bool save_settings_in_robot;
00216 
00217   int param_idx;  // index in the RobotParams table for this robot
00218 
00219   // Max motor speeds (mm/sec,deg/sec)
00220   int motor_max_speed;
00221   int motor_max_turnspeed;
00222 
00223   // Customized control settings for the robot
00224   int16_t pid_trans_p, pid_trans_v, pid_trans_i;
00225   int16_t pid_rot_p, pid_rot_v, pid_rot_i;
00226 
00227   // This is a fairly low-level setting that is exposed
00228   uint16_t motor_pwm_frequency, motor_pwm_max_on;
00229 
00230   // Bound the command velocities
00231   bool use_vel_band;
00232 
00233   // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
00234   short motor_max_trans_accel, motor_max_trans_decel;
00235   short motor_max_rot_accel, motor_max_rot_decel;
00236 
00237 public:
00238 
00239   Erratic(ConfigFile* cf, int section);
00240 
00241   virtual int Subscribe(player_devaddr_t id);
00242   virtual int Unsubscribe(player_devaddr_t id);
00243 
00244   /* the main thread */
00245   virtual void Main();
00246 
00247   virtual int Setup();
00248   virtual int Shutdown();
00249 
00250   // MessageHandler
00251   virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
00252 };
00253 
00254 
00255 #endif