MorphGI / Mbed OS MGI_Rebuild_MidLevel_9_0-Basic_PWM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ros_lib_kinetic

HLComms.cpp@29:10a5cf37a875, 2019-02-08 (annotated)

Committer:
WD40andTape
Date:
Fri Feb 08 18:36:15 2019 +0000
Revision:
29:10a5cf37a875
Parent:
28:8e0c502c1a50
Child:
30:6c8eea90735e
Actuator numbering standardised. Units converted to mm. Added text message feedback. Using Int16MultiArray instead of Float32MultiArray.

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 28:8e0c502c1a50 19 delete *_duration_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 29:10a5cf37a875 29 ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
WD40andTape 28:8e0c502c1a50 30 _nh.subscribe(demands_sub);
WD40andTape 28:8e0c502c1a50 31
WD40andTape 29:10a5cf37a875 32 _text_pub = new ros::Publisher("ML_TextOut", &_text_msg);
WD40andTape 29:10a5cf37a875 33 _nh.advertise(*_text_pub);
WD40andTape 29:10a5cf37a875 34
WD40andTape 28:8e0c502c1a50 35 _sensor_msg.data_length = N_CHANNELS*2;
WD40andTape 28:8e0c502c1a50 36 _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
WD40andTape 28:8e0c502c1a50 37 _nh.advertise(*_sensor_pub);
WD40andTape 28:8e0c502c1a50 38
WD40andTape 28:8e0c502c1a50 39 _duration_pub = new ros::Publisher("ML_Duration", &_duration_msg);
WD40andTape 28:8e0c502c1a50 40 _nh.advertise(*_duration_pub);
WD40andTape 28:8e0c502c1a50 41
WD40andTape 28:8e0c502c1a50 42 wait_ms(1000);
WD40andTape 28:8e0c502c1a50 43 _rosReady.release();
WD40andTape 28:8e0c502c1a50 44
WD40andTape 28:8e0c502c1a50 45 Ticker spinTicker;
WD40andTape 28:8e0c502c1a50 46 spinTicker.attach(callback(this,&HLComms::releaseSemSpin), 1/(float)_freq_hz); // Set up HL comms to recur at fixed intervals
WD40andTape 28:8e0c502c1a50 47
WD40andTape 28:8e0c502c1a50 48 while (1) {
WD40andTape 28:8e0c502c1a50 49 _semSpin.wait();
WD40andTape 28:8e0c502c1a50 50 _nh.spinOnce();
WD40andTape 7:5b6a2cefbf3b 51 }
WD40andTape 7:5b6a2cefbf3b 52 }
WD40andTape 7:5b6a2cefbf3b 53
WD40andTape 28:8e0c502c1a50 54 void HLComms::releaseSemSpin(void) {
WD40andTape 28:8e0c502c1a50 55 _semSpin.release();
WD40andTape 9:cd3607ba5643 56 }
WD40andTape 9:cd3607ba5643 57
WD40andTape 28:8e0c502c1a50 58 // INPUT
WD40andTape 9:cd3607ba5643 59
WD40andTape 29:10a5cf37a875 60 void HLComms::receive_demands(const std_msgs::Int16MultiArray &demands_array) {
WD40andTape 28:8e0c502c1a50 61 struct demands_struct _protected_input;
WD40andTape 28:8e0c502c1a50 62 recv_mutex.lock();
WD40andTape 29:10a5cf37a875 63 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 29:10a5cf37a875 64 _protected_input.psi_mm[i] = MAX_ACTUATOR_LIMIT_MM*((double)demands_array.data[i]/511);
WD40andTape 29:10a5cf37a875 65 _protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 7:5b6a2cefbf3b 66 }
WD40andTape 29:10a5cf37a875 67 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
WD40andTape 29:10a5cf37a875 68 //short int demands_array_length = sizeof(demands_array.data)/sizeof(demands_array.data[0]);
WD40andTape 29:10a5cf37a875 69 //_protected_input.speed_mmps = demands_array.data[demands_array_length-1];
WD40andTape 29:10a5cf37a875 70 _protected_input.speed_mmps = MAX_SPEED_MMPS*((double)demands_array.data[9]/511);
WD40andTape 29:10a5cf37a875 71 _protected_input.speed_mmps = min( max(_protected_input.speed_mmps,0.0) , (double)MAX_SPEED_MMPS );
WD40andTape 28:8e0c502c1a50 72 recv_mutex.unlock();
WD40andTape 28:8e0c502c1a50 73 _input = _protected_input;
WD40andTape 29:10a5cf37a875 74 /*char array[20];
WD40andTape 29:10a5cf37a875 75 sprintf(array, "Speed: %f", _input.speed_mmps);
WD40andTape 29:10a5cf37a875 76 send_text_message(array);
WD40andTape 29:10a5cf37a875 77 sprintf(array, "Psi[0]: %f", _input.psi_mm[0]);
WD40andTape 29:10a5cf37a875 78 send_text_message(array);
WD40andTape 29:10a5cf37a875 79 sprintf(array, "Psi[1]: %f", _input.psi_mm[1]);
WD40andTape 29:10a5cf37a875 80 send_text_message(array);*/
WD40andTape 28:8e0c502c1a50 81 newData.release();
WD40andTape 17:bbaf3e8440ad 82 }
WD40andTape 17:bbaf3e8440ad 83
WD40andTape 28:8e0c502c1a50 84 demands_struct HLComms::get_demands(void) {
WD40andTape 28:8e0c502c1a50 85 return _input;
WD40andTape 28:8e0c502c1a50 86 }
WD40andTape 28:8e0c502c1a50 87
WD40andTape 28:8e0c502c1a50 88 // OUTPUT
WD40andTape 28:8e0c502c1a50 89
WD40andTape 29:10a5cf37a875 90 void HLComms::send_text_message(char text[]) {
WD40andTape 29:10a5cf37a875 91 _text_msg.data = text;
WD40andTape 29:10a5cf37a875 92 _text_pub->publish(&_text_msg);
WD40andTape 29:10a5cf37a875 93 }
WD40andTape 29:10a5cf37a875 94
WD40andTape 28:8e0c502c1a50 95 void HLComms::send_duration_message(double dblTime) {
WD40andTape 28:8e0c502c1a50 96 _duration_msg.data = dblTime;
WD40andTape 28:8e0c502c1a50 97 _duration_pub->publish(&_duration_msg);
WD40andTape 9:cd3607ba5643 98 }
WD40andTape 9:cd3607ba5643 99
WD40andTape 29:10a5cf37a875 100 void HLComms::send_sensor_message(unsigned int positions[], unsigned int pressures[]) {
WD40andTape 28:8e0c502c1a50 101 short int i_channel;
WD40andTape 28:8e0c502c1a50 102 for(short int i=0; i<N_CHANNELS*2; i++) {
WD40andTape 28:8e0c502c1a50 103 i_channel = i%N_CHANNELS;
WD40andTape 28:8e0c502c1a50 104 if(i<N_CHANNELS) {
WD40andTape 28:8e0c502c1a50 105 _sensor_msg.data[i] = positions[i_channel];
WD40andTape 28:8e0c502c1a50 106 } else {
WD40andTape 28:8e0c502c1a50 107 _sensor_msg.data[i] = pressures[i_channel];
WD40andTape 28:8e0c502c1a50 108 }
WD40andTape 28:8e0c502c1a50 109 }
WD40andTape 28:8e0c502c1a50 110 _sensor_pub->publish(&_sensor_msg);
WD40andTape 9:cd3607ba5643 111 }