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

Last updated 12 September 2005 21:38:45