00001 #ifndef MULTICAR_HYDRAULIC_CORE_H_ 00002 #define MULTICAR_HYDRAULIC_CORE_H_ 00003 00004 #include <string> 00005 #include <stdint.h> 00006 #include <stdio.h> 00007 #include "multicar_hydraulic/CanCmd.h" 00008 #include "tinycan/CanMsg.h" 00009 #include "sensor_msgs/JointState.h" 00010 00011 namespace hydraulic { 00012 00013 // hydraulic pre control states 00014 #define PRE_CONTROL_SIG_ON 1 00015 #define PRE_CONTROL_SIG_ON_ACK 2 00016 #define PRE_CONTROL_SIG_OFF 3 00017 #define PRE_CONTROL_SIG_OFF_ACK 4 00018 00019 // hydraulic states 00020 #define HYDRAULIC_HA_NA 0 00021 #define HYDRAULIC_NA_VS 1 00022 #define HYDRAULIC_HA_SWK 2 00023 00024 // hydraulic movements 00025 #define MOVE_HALT 0 00026 #define MOVE_MOVING 1 00027 #define MOVE_THRUST_RIGHT 2 00028 #define MOVE_THRUST_LEFT 3 00029 #define MOVE_UP 2 00030 #define MOVE_DOWN 3 00031 00032 // sensors 00033 #define CO_NMT_MSG 0x00 00034 #define CO_VALUES_MSG 0x180 00035 #define CO_BOOM_MSG 0x280 00036 #define CO_HEARTBEAT_MSG 0x700 00037 #define CO_SYNC_MSG 0x80 00038 00039 // hydraulic 00040 #define CO_HYDRAULIC_ACK 0x180 00041 #define CO_PRE_CONTROL 0x200 00042 00043 #define NID_Hydraulik 20 00044 #define NID_Schnellwechselkopf 125 00045 #define NID_Nebenarm 126 00046 #define NID_Hauptarm 127 00047 #define NID_THRUST 128 // only virtual 00048 #define NID_Schwenkfix 129 // only virtual 00049 00050 #define IDX_Schnellwechselkopf 0 00051 #define IDX_Nebenarm 1 00052 #define IDX_Hauptarm 2 00053 #define IDX_VERSCHUB 3 00054 #define IDX_Schwenkfix 4 00055 00056 struct Ausleger { 00057 static uint8_t pre_control_signal; 00058 uint16_t node_id; 00059 std::string node_name; 00060 uint8_t state; 00061 float position; 00062 int16_t velocity; 00063 uint32_t target_position; 00064 uint32_t target_velocity; 00065 bool is_moveing; 00066 void setValues(uint32_t position, int16_t speed) 00067 { 00068 this->position = position * 100; // µm 00069 this->position = roundf(this->position / 1000.0); // µm to mm 00070 this->velocity = speed; // mm/s 00071 } 00072 void printValues() 00073 { 00074 printf("Node %d (%s) :: Position:%f mm :: Velocity:%d mm/s\n", node_id, 00075 node_name.c_str(), position, velocity); 00076 } 00077 }; 00078 00079 class Hydraulic { 00080 00081 public: 00083 Hydraulic(); 00085 ~Hydraulic(); 00087 int init(); 00089 void publishCanMessage(tinycan::CanMsg *msg); 00091 void publishJointStateMessage(int nid, uint32_t pos, uint16_t velo); 00093 void callbackCanMessageRaw(const tinycan::CanMsg::ConstPtr& msg); 00095 void callbackRobotTrajectory(const sensor_msgs::JointState::ConstPtr& msg); 00097 int checkTask(); 00099 void setCanMsgPublisher(ros::Publisher *p) { cpub = p; } 00101 void setJointStatePublisher(ros::Publisher *p) { jspub = p; } 00103 void cmdToCanMessage(std::string cmd, int val); 00105 void setPWM(uint8_t nid, uint16_t pwm); 00106 private: 00107 Ausleger ausleger[5]; 00108 uint8_t hydraulic_states[3][8]; 00109 ros::Publisher *cpub; 00110 ros::Publisher *jspub; 00111 bool is_man_ctr; 00112 bool is_man_ctr_act; 00113 }; 00114 } 00115 #endif