Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.
Dependencies: ros_lib_kinetic
HLComms.h
00001 // HLComms.h 00002 00003 #ifndef HLCOMMS_H 00004 #define HLCOMMS_H 00005 00006 // STANDARD IMPORTS 00007 #include "math.h" 00008 #include <algorithm> 00009 #include <limits> 00010 // MBED IMPORTS 00011 #include"mbed.h" 00012 #include "mbed_events.h" 00013 // ROS IMPORTS 00014 #include <ros.h> 00015 #include <std_msgs/String.h> 00016 #include <std_msgs/Float32.h> 00017 #include <std_msgs/Int16MultiArray.h> 00018 #include <std_msgs/Float32MultiArray.h> 00019 #include "std_msgs/MultiArrayLayout.h" 00020 #include "std_msgs/MultiArrayDimension.h" 00021 // CUSTOM IMPORTS 00022 #include "MLSettings.h" 00023 00024 struct demands_struct { 00025 double psi_mm[N_CHANNELS]; 00026 double speeds_mmps[3]; 00027 bool utitilies_bool[4]; 00028 }; 00029 00030 class HLComms 00031 { 00032 public: 00033 00034 Semaphore newData; 00035 00036 HLComms(unsigned short int freq_hz); 00037 //~HLComms(void); 00038 demands_struct get_demands(void); 00039 void send_text_message(char text[]); 00040 void send_durations_message(double dblTimes[]); 00041 void send_sensor_message(unsigned int positions[], unsigned int pressures[]); 00042 00043 private: 00044 00045 unsigned short int _freq_hz; 00046 struct demands_struct _input; 00047 00048 Thread _threadROS; 00049 Semaphore _rosReady; 00050 Semaphore _semSpin; 00051 ros::NodeHandle _nh; 00052 ros::Publisher* _text_pub; 00053 std_msgs::String _text_msg; 00054 std_msgs::Int16MultiArray _sensor_msg; 00055 ros::Publisher* _sensor_pub; 00056 std_msgs::Float32MultiArray _durations_msg; 00057 ros::Publisher* _durations_pub; 00058 00059 Mutex recv_mutex; 00060 00061 void ros_main(void); 00062 void releaseSemSpin(void); 00063 void receive_demands(const std_msgs::Float32MultiArray &demands_array); 00064 00065 }; 00066 00067 #endif
Generated on Wed Dec 6 2023 18:04:00 by
1.7.2