Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.

Dependencies:   ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers HLComms.cpp Source File

HLComms.cpp

00001 // HLComms.cpp
00002 
00003 #include "HLComms.h"
00004 
00005 HLComms::HLComms(unsigned short int freq_hz) : // Constructor
00006     newData(1),
00007     _freq_hz(freq_hz),
00008     _threadROS(osPriorityBelowNormal),
00009     _rosReady(1),
00010     _semSpin(1)
00011 {
00012     _threadROS.start(callback(this, &HLComms::ros_main));// Start ROS thread
00013     _rosReady.wait();
00014 }
00015 
00016 /*HLComms::~HLComms(void) { // Destructor
00017     delete *_text_pub;
00018     delete *_sensor_pub;
00019     delete *_durations_pub;
00020 }*/
00021 
00022 // ROS
00023 
00024 void HLComms::ros_main(void) {
00025     _nh.getHardware()->setBaud(BAUD_RATE);
00026     _nh.initNode();
00027     wait_ms(1000);
00028     
00029     //ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
00030     ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
00031     _nh.subscribe(demands_sub);
00032     
00033     _text_pub = new ros::Publisher("ML_TextOut", &_text_msg);
00034     _nh.advertise(*_text_pub);
00035     
00036     _sensor_msg.data_length = N_CHANNELS*2;
00037     _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
00038     _nh.advertise(*_sensor_pub);
00039     
00040     _durations_msg.data_length = 3;
00041     _durations_pub = new ros::Publisher("ML_Durations", &_durations_msg);
00042     _nh.advertise(*_durations_pub);
00043     
00044     wait_ms(1000);
00045     _rosReady.release();
00046     
00047     Ticker spinTicker;
00048     spinTicker.attach(callback(this,&HLComms::releaseSemSpin), 1/(float)_freq_hz); // Set up HL comms to recur at fixed intervals
00049     
00050     while (1) {
00051         _semSpin.wait();
00052         _nh.spinOnce();
00053     }
00054 }
00055 
00056 void HLComms::releaseSemSpin(void) {
00057     _semSpin.release();
00058 }
00059 
00060 // INPUT
00061 
00062 void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) {
00063     struct demands_struct _protected_input;
00064     recv_mutex.lock();
00065     for(int i=0; i<N_CHANNELS; i++) {
00066         _protected_input.psi_mm[i] = (double)demands_array.data[i];
00067         //_protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
00068     }
00069     _protected_input.speeds_mmps[0] = (double)demands_array.data[9];
00070     _protected_input.speeds_mmps[0] = min( max(_protected_input.speeds_mmps[0],0.0) , (double)MAX_SPEED_MMPS );
00071     _protected_input.speeds_mmps[1] = (double)demands_array.data[10];
00072     _protected_input.speeds_mmps[1] = min( max(_protected_input.speeds_mmps[1],0.0) , (double)MAX_SPEED_MMPS );
00073     _protected_input.speeds_mmps[2] = (double)demands_array.data[11];
00074     _protected_input.speeds_mmps[2] = min( max(_protected_input.speeds_mmps[2],0.0) , (double)MAX_SPEED_MMPS );
00075     _protected_input.utitilies_bool[0] = fabs(demands_array.data[12]) > numeric_limits<float>::epsilon();
00076     _protected_input.utitilies_bool[1] = fabs(demands_array.data[13]) > numeric_limits<float>::epsilon();
00077     _protected_input.utitilies_bool[2] = fabs(demands_array.data[14]) > numeric_limits<float>::epsilon();
00078     _protected_input.utitilies_bool[3] = fabs(demands_array.data[15]) > numeric_limits<float>::epsilon();
00079     
00080     recv_mutex.unlock();
00081     _input = _protected_input;
00082     newData.release();
00083 }
00084 
00085 demands_struct HLComms::get_demands(void) {
00086     return _input;
00087 }
00088 
00089 // OUTPUT
00090 
00091 void HLComms::send_text_message(char text[]) {
00092     _text_msg.data = text;
00093     _text_pub->publish(&_text_msg);
00094 }
00095 
00096 void HLComms::send_durations_message(double dblTimes[]) {
00097     for(short int i=0; i<3; i++) {
00098         _durations_msg.data[i] = dblTimes[i];
00099     }
00100     _durations_pub->publish(&_durations_msg);
00101 }
00102 
00103 void HLComms::send_sensor_message(unsigned int positions[], unsigned int pressures[]) {
00104     short int i_channel;
00105     for(short int i=0; i<N_CHANNELS*2; i++) {
00106         i_channel = i%N_CHANNELS;
00107         if(i<N_CHANNELS) {
00108             _sensor_msg.data[i] = positions[i_channel];
00109         } else {
00110             _sensor_msg.data[i] = pressures[i_channel];
00111         }
00112     }
00113     _sensor_pub->publish(&_sensor_msg);
00114 }