Go to the documentation of this file.00001 #ifndef IXIS_IMCS01_DRIVER_H
00002 #define IXIS_IMCS01_DRIVER_H
00003
00004 #include <ros/ros.h>
00005 #include <sensor_msgs/JointState.h>
00006
00007 #include <vector>
00008 #include <string>
00009 #include <mutex>
00010
00011 #include <termios.h>
00012 #include "urbtc.h"
00013 #include "urobotc.h"
00014
00015 #define JOINT_INDEX_REAR_RIGHT 0
00016 #define JOINT_INDEX_REAR_LEFT 1
00017 #define JOINT_INDEX_FRONT 2
00018
00019 template<typename N, typename M>
00020 inline double MIN(const N& a, const M& b)
00021 {
00022 return a < b ? a : b;
00023 }
00024
00025 template<typename N, typename M>
00026 inline double MAX(const N& a, const M& b)
00027 {
00028 return a > b ? a : b;
00029 }
00030
00031 template<typename T>
00032 inline double NORMALIZE(const T& z)
00033 {
00034 return atan2(sin(z), cos(z));
00035 }
00036
00037 enum class RunningState {
00038 FORWARD,
00039 FORWARD_STOP,
00040 BACK,
00041 BACK_STOP,
00042 OTHERWISE
00043 };
00044
00045
00046 enum class RunningMode {
00047 FORWARD,
00048 BACK
00049 };
00050
00051
00052 class IxisImcs01Driver
00053 {
00054 public:
00055 IxisImcs01Driver(std::string port_name);
00056 ~IxisImcs01Driver();
00057 int update();
00058 sensor_msgs::JointState getJointState();
00059 int controlRearWheel(double rear_speed);
00060 protected:
00061 int openPort(std::string port_name);
00062 int closePort();
00063 int setImcs01();
00064 int parseEncoderTime();
00065 int parseFrontEncoderCounts();
00066 int parseRearEncoderCounts();
00067 int writeOffsetCmd(RunningMode mode,
00068 unsigned short duty);
00069 struct uin received_data_;
00070 std::vector<int> rear_last_encoder_counts_;
00071 std::vector<int> rear_delta_encoder_counts_;
00072 double delta_encoder_time_;
00073 double last_encoder_time_;
00074 sensor_msgs::JointState state_;
00075 int imcs01_fd_;
00076 RunningState running_state_;
00077 struct ccmd cmd_ccmd_;
00078 struct uin cmd_uin_;
00079 termios oldtio_imcs01_;
00080 termios newtio_imcs01_;
00081 std::mutex communication_mutex_;
00082 };
00083
00084 #endif