ixis_imcs01_driver.h
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 // RunningMode means switch of Handle of car.
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 /* IXIS_IMCS01_DRIVER_H */


cirkit_unit03_base
Author(s): CIR-KIT
autogenerated on Thu Jun 6 2019 21:08:13