devsteer.h
Go to the documentation of this file.00001 /* 00002 * ART steering servo controller device interface 00003 * 00004 * Copyright (C) 2008 Austin Robot Technology 00005 * 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id: devsteer.h 1161 2011-03-26 02:10:49Z jack.oquin $ 00009 */ 00010 00011 00040 #ifndef _DEVSTEER_HH_ 00041 #define _DEVSTEER_HH_ 00042 00043 #include <errno.h> 00044 #include <stdio.h> 00045 #include <stdint.h> 00046 #include <math.h> 00047 00048 #include <art/conversions.h> // A/D conversion 00049 #include <art_msgs/ArtVehicle.h> 00050 #include <art_msgs/SteeringDiagnostics.h> 00051 00052 #include "../servo.h" 00053 00054 #define DEVICE "Quicksilver" 00055 00056 // QuickSilver Stepper (31 degrees at 310,000 ticks) 00057 // 00058 // The vehicle's steering linkage has an actual range of plus or minus 00059 // 31 degrees. We limit our requests to plus or minus 29 degrees, to 00060 // prevent mechanical damage. 00061 // 00062 // theoretical value: (-10000.0) 00063 //#define TICKS_PER_DEGREE ((float)(-310000.0 / 31.0)) 00064 // 00065 // more exact value determined by curve fit of measurements: 00066 #define TICKS_PER_DEGREE ((double) -9841.65332031) 00067 00068 #define DBG(format,args...) ROS_DEBUG(format, ## args) 00069 00070 // One per steering hardware device. 00071 class devsteer: Servo 00072 { 00073 public: 00074 00075 devsteer(int32_t center=0); 00076 ~devsteer() {}; 00077 00078 int Open(); 00079 int Close(); 00080 int Configure(); 00081 00082 // Quicksilver command methods 00083 int check_status(void); 00084 int get_angle(float °rees); 00085 void publish_diag(const ros::Publisher &diag_pub); 00086 int set_initial_angle(float position); 00087 int steering_absolute(float position); 00088 int steering_relative(float position); 00089 00090 // convert steering angle to encoder ticks 00091 inline int32_t degrees2ticks(float degrees) 00092 { 00093 return (int32_t) lrint(degrees * TICKS_PER_DEGREE) + center_ticks_; 00094 } 00095 00096 // convert encoder ticks to steering angle 00097 inline float ticks2degrees(int32_t ticks) 00098 { 00099 return (ticks - center_ticks_) / TICKS_PER_DEGREE; 00100 } 00101 00102 private: 00103 00104 int64_t GetTime(); 00105 00106 // configuration options 00107 bool center_on_exit_; // center steering during shutdown() 00108 std::string port_; // tty port name 00109 bool training_; // use training mode 00110 bool simulate_moving_errors_; // simulate intermittent moving errors 00111 double steering_rate_; // steering velocity (deg/sec) 00112 00113 float req_angle_; // requested angle (absolute) 00114 float starting_angle_; // starting wheel angle 00115 int32_t starting_ticks_; // starting wheel encoder ticks 00116 int32_t center_ticks_; // center wheel encoder ticks 00117 00118 art_msgs::SteeringDiagnostics diag_msg_; 00119 00120 int configure_steering(void); 00121 int encoder_goto(float degrees); 00122 int get_encoder(int32_t &iticks); 00123 int get_status_word(uint16_t &status); 00124 int send_cmd(const char *string); 00125 int servo_cmd(const char *string); 00126 void servo_write_only(const char *string); 00127 int write_register(int reg, int32_t val); 00128 00129 }; 00130 00131 // limit position value to normal range 00132 static inline float limit_travel(float position) 00133 { 00134 if (position > art_msgs::ArtVehicle::max_steer_degrees) 00135 position = art_msgs::ArtVehicle::max_steer_degrees; 00136 else if (position < -art_msgs::ArtVehicle::max_steer_degrees) 00137 position = -art_msgs::ArtVehicle::max_steer_degrees; 00138 return position; 00139 } 00140 00141 #endif // _DEVSTEER_HH_