MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

HLComms.cpp@33:9877ca32e43c, 2019-03-13 (annotated)

Committer:
WD40andTape
Date:
Wed Mar 13 11:59:58 2019 +0000
Revision:
33:9877ca32e43c
Parent:
32:8c59c536a2a6
Child:
36:4459be8296e9
Added support functions - receiving and control.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // HLComms.cpp
WD40andTape 7:5b6a2cefbf3b 2
WD40andTape 7:5b6a2cefbf3b 3 #include "HLComms.h"
WD40andTape 7:5b6a2cefbf3b 4
WD40andTape 28:8e0c502c1a50 5 HLComms::HLComms(unsigned short int freq_hz) : // Constructor
WD40andTape 28:8e0c502c1a50 6 newData(1),
WD40andTape 28:8e0c502c1a50 7 _freq_hz(freq_hz),
WD40andTape 28:8e0c502c1a50 8 _threadROS(osPriorityBelowNormal),
WD40andTape 28:8e0c502c1a50 9 _rosReady(1),
WD40andTape 28:8e0c502c1a50 10 _semSpin(1)
WD40andTape 28:8e0c502c1a50 11 {
WD40andTape 28:8e0c502c1a50 12 _threadROS.start(callback(this, &HLComms::ros_main));// Start ROS thread
WD40andTape 28:8e0c502c1a50 13 _rosReady.wait();
WD40andTape 7:5b6a2cefbf3b 14 }
WD40andTape 7:5b6a2cefbf3b 15
WD40andTape 28:8e0c502c1a50 16 /*HLComms::~HLComms(void) { // Destructor
WD40andTape 29:10a5cf37a875 17 delete *_text_pub;
WD40andTape 28:8e0c502c1a50 18 delete *_sensor_pub;
WD40andTape 31:08cb04eb75fc 19 delete *_durations_pub;
WD40andTape 28:8e0c502c1a50 20 }*/
WD40andTape 28:8e0c502c1a50 21
WD40andTape 28:8e0c502c1a50 22 // ROS
WD40andTape 28:8e0c502c1a50 23
WD40andTape 28:8e0c502c1a50 24 void HLComms::ros_main(void) {
WD40andTape 28:8e0c502c1a50 25 _nh.getHardware()->setBaud(BAUD_RATE);
WD40andTape 28:8e0c502c1a50 26 _nh.initNode();
WD40andTape 28:8e0c502c1a50 27 wait_ms(1000);
WD40andTape 28:8e0c502c1a50 28
WD40andTape 30:6c8eea90735e 29 //ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
WD40andTape 30:6c8eea90735e 30 ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
WD40andTape 28:8e0c502c1a50 31 _nh.subscribe(demands_sub);
WD40andTape 28:8e0c502c1a50 32
WD40andTape 29:10a5cf37a875 33 _text_pub = new ros::Publisher("ML_TextOut", &_text_msg);
WD40andTape 29:10a5cf37a875 34 _nh.advertise(*_text_pub);
WD40andTape 29:10a5cf37a875 35
WD40andTape 28:8e0c502c1a50 36 _sensor_msg.data_length = N_CHANNELS*2;
WD40andTape 28:8e0c502c1a50 37 _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
WD40andTape 28:8e0c502c1a50 38 _nh.advertise(*_sensor_pub);
WD40andTape 28:8e0c502c1a50 39
WD40andTape 31:08cb04eb75fc 40 _durations_msg.data_length = 3;
WD40andTape 31:08cb04eb75fc 41 _durations_pub = new ros::Publisher("ML_Durations", &_durations_msg);
WD40andTape 31:08cb04eb75fc 42 _nh.advertise(*_durations_pub);
WD40andTape 28:8e0c502c1a50 43
WD40andTape 28:8e0c502c1a50 44 wait_ms(1000);
WD40andTape 28:8e0c502c1a50 45 _rosReady.release();
WD40andTape 28:8e0c502c1a50 46
WD40andTape 28:8e0c502c1a50 47 Ticker spinTicker;
WD40andTape 28:8e0c502c1a50 48 spinTicker.attach(callback(this,&HLComms::releaseSemSpin), 1/(float)_freq_hz); // Set up HL comms to recur at fixed intervals
WD40andTape 28:8e0c502c1a50 49
WD40andTape 28:8e0c502c1a50 50 while (1) {
WD40andTape 28:8e0c502c1a50 51 _semSpin.wait();
WD40andTape 28:8e0c502c1a50 52 _nh.spinOnce();
WD40andTape 7:5b6a2cefbf3b 53 }
WD40andTape 7:5b6a2cefbf3b 54 }
WD40andTape 7:5b6a2cefbf3b 55
WD40andTape 28:8e0c502c1a50 56 void HLComms::releaseSemSpin(void) {
WD40andTape 28:8e0c502c1a50 57 _semSpin.release();
WD40andTape 9:cd3607ba5643 58 }
WD40andTape 9:cd3607ba5643 59
WD40andTape 28:8e0c502c1a50 60 // INPUT
WD40andTape 9:cd3607ba5643 61
WD40andTape 30:6c8eea90735e 62 /*void HLComms::receive_demands(const std_msgs::Int16MultiArray &demands_array) {
WD40andTape 28:8e0c502c1a50 63 struct demands_struct _protected_input;
WD40andTape 28:8e0c502c1a50 64 recv_mutex.lock();
WD40andTape 29:10a5cf37a875 65 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 30:6c8eea90735e 66 _protected_input.psi_mm[i] = ((MAX_ACTUATOR_LIMIT_MM+1.0)*((double)demands_array.data[i]/65535)-1.0);
WD40andTape 30:6c8eea90735e 67 if( _protected_input.psi_mm[i]<-0.5 ) {
WD40andTape 30:6c8eea90735e 68 _protected_input.psi_mm[i] = -1.0;
WD40andTape 30:6c8eea90735e 69 } else {
WD40andTape 30:6c8eea90735e 70 _protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 30:6c8eea90735e 71 }
WD40andTape 7:5b6a2cefbf3b 72 }
WD40andTape 29:10a5cf37a875 73 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
WD40andTape 29:10a5cf37a875 74 //short int demands_array_length = sizeof(demands_array.data)/sizeof(demands_array.data[0]);
WD40andTape 29:10a5cf37a875 75 //_protected_input.speed_mmps = demands_array.data[demands_array_length-1];
WD40andTape 30:6c8eea90735e 76 _protected_input.speed_mmps = MAX_SPEED_MMPS*((double)demands_array.data[9]/65535);
WD40andTape 29:10a5cf37a875 77 _protected_input.speed_mmps = min( max(_protected_input.speed_mmps,0.0) , (double)MAX_SPEED_MMPS );
WD40andTape 28:8e0c502c1a50 78 recv_mutex.unlock();
WD40andTape 28:8e0c502c1a50 79 _input = _protected_input;
WD40andTape 30:6c8eea90735e 80 //char array[20];
WD40andTape 30:6c8eea90735e 81 //sprintf(array, "Speed: %f", _input.speed_mmps);
WD40andTape 30:6c8eea90735e 82 //send_text_message(array);
WD40andTape 30:6c8eea90735e 83 newData.release();
WD40andTape 30:6c8eea90735e 84 }*/
WD40andTape 30:6c8eea90735e 85
WD40andTape 30:6c8eea90735e 86 void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) {
WD40andTape 30:6c8eea90735e 87 struct demands_struct _protected_input;
WD40andTape 30:6c8eea90735e 88 recv_mutex.lock();
WD40andTape 30:6c8eea90735e 89 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 30:6c8eea90735e 90 _protected_input.psi_mm[i] = (double)demands_array.data[i];
WD40andTape 32:8c59c536a2a6 91 //_protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 30:6c8eea90735e 92 }
WD40andTape 32:8c59c536a2a6 93 // demands_array.data[8] ignored
WD40andTape 31:08cb04eb75fc 94 _protected_input.speeds_mmps[0] = (double)demands_array.data[9];
WD40andTape 31:08cb04eb75fc 95 _protected_input.speeds_mmps[0] = min( max(_protected_input.speeds_mmps[0],0.0) , (double)MAX_SPEED_MMPS );
WD40andTape 31:08cb04eb75fc 96 _protected_input.speeds_mmps[1] = (double)demands_array.data[10];
WD40andTape 31:08cb04eb75fc 97 _protected_input.speeds_mmps[1] = min( max(_protected_input.speeds_mmps[1],0.0) , (double)MAX_SPEED_MMPS );
WD40andTape 31:08cb04eb75fc 98 _protected_input.speeds_mmps[2] = (double)demands_array.data[11];
WD40andTape 31:08cb04eb75fc 99 _protected_input.speeds_mmps[2] = min( max(_protected_input.speeds_mmps[2],0.0) , (double)MAX_SPEED_MMPS );
WD40andTape 33:9877ca32e43c 100 _protected_input.utitilies_bool[0] = fabs(demands_array.data[12]) > numeric_limits<float>::epsilon();
WD40andTape 33:9877ca32e43c 101 _protected_input.utitilies_bool[1] = fabs(demands_array.data[13]) > numeric_limits<float>::epsilon();
WD40andTape 33:9877ca32e43c 102 _protected_input.utitilies_bool[2] = fabs(demands_array.data[14]) > numeric_limits<float>::epsilon();
WD40andTape 33:9877ca32e43c 103 _protected_input.utitilies_bool[3] = fabs(demands_array.data[15]) > numeric_limits<float>::epsilon();
WD40andTape 33:9877ca32e43c 104
WD40andTape 30:6c8eea90735e 105 recv_mutex.unlock();
WD40andTape 30:6c8eea90735e 106 _input = _protected_input;
WD40andTape 28:8e0c502c1a50 107 newData.release();
WD40andTape 17:bbaf3e8440ad 108 }
WD40andTape 17:bbaf3e8440ad 109
WD40andTape 28:8e0c502c1a50 110 demands_struct HLComms::get_demands(void) {
WD40andTape 28:8e0c502c1a50 111 return _input;
WD40andTape 28:8e0c502c1a50 112 }
WD40andTape 28:8e0c502c1a50 113
WD40andTape 28:8e0c502c1a50 114 // OUTPUT
WD40andTape 28:8e0c502c1a50 115
WD40andTape 29:10a5cf37a875 116 void HLComms::send_text_message(char text[]) {
WD40andTape 29:10a5cf37a875 117 _text_msg.data = text;
WD40andTape 29:10a5cf37a875 118 _text_pub->publish(&_text_msg);
WD40andTape 29:10a5cf37a875 119 }
WD40andTape 29:10a5cf37a875 120
WD40andTape 31:08cb04eb75fc 121 void HLComms::send_durations_message(double dblTimes[]) {
WD40andTape 31:08cb04eb75fc 122 for(short int i=0; i<3; i++) {
WD40andTape 31:08cb04eb75fc 123 _durations_msg.data[i] = dblTimes[i];
WD40andTape 31:08cb04eb75fc 124 }
WD40andTape 31:08cb04eb75fc 125 _durations_pub->publish(&_durations_msg);
WD40andTape 9:cd3607ba5643 126 }
WD40andTape 9:cd3607ba5643 127
WD40andTape 29:10a5cf37a875 128 void HLComms::send_sensor_message(unsigned int positions[], unsigned int pressures[]) {
WD40andTape 28:8e0c502c1a50 129 short int i_channel;
WD40andTape 28:8e0c502c1a50 130 for(short int i=0; i<N_CHANNELS*2; i++) {
WD40andTape 28:8e0c502c1a50 131 i_channel = i%N_CHANNELS;
WD40andTape 28:8e0c502c1a50 132 if(i<N_CHANNELS) {
WD40andTape 28:8e0c502c1a50 133 _sensor_msg.data[i] = positions[i_channel];
WD40andTape 28:8e0c502c1a50 134 } else {
WD40andTape 28:8e0c502c1a50 135 _sensor_msg.data[i] = pressures[i_channel];
WD40andTape 28:8e0c502c1a50 136 }
WD40andTape 28:8e0c502c1a50 137 }
WD40andTape 28:8e0c502c1a50 138 _sensor_pub->publish(&_sensor_msg);
WD40andTape 9:cd3607ba5643 139 }