Diff: HLComms.cpp

Revision:

29:10a5cf37a875

Parent:

28:8e0c502c1a50

Child:

30:6c8eea90735e

--- a/HLComms.cpp	Wed Feb 06 16:10:18 2019 +0000
+++ b/HLComms.cpp	Fri Feb 08 18:36:15 2019 +0000
@@ -14,6 +14,7 @@
 }
 
 /*HLComms::~HLComms(void) { // Destructor
+    delete *_text_pub;
     delete *_sensor_pub;
     delete *_duration_pub;
 }*/
@@ -21,14 +22,16 @@
 // ROS
 
 void HLComms::ros_main(void) {
-    //printf("HELLO\r\n");
     _nh.getHardware()->setBaud(BAUD_RATE);
     _nh.initNode();
     wait_ms(1000);
     
-    ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
+    ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
     _nh.subscribe(demands_sub);
     
+    _text_pub = new ros::Publisher("ML_TextOut", &_text_msg);
+    _nh.advertise(*_text_pub);
+    
     _sensor_msg.data_length = N_CHANNELS*2;
     _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
     _nh.advertise(*_sensor_pub);
@@ -54,26 +57,27 @@
 
 // INPUT
 
-void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) {
+void HLComms::receive_demands(const std_msgs::Int16MultiArray &demands_array) {
     struct demands_struct _protected_input;
     recv_mutex.lock();
-    /*for(int i=0; i<N_CHANNELS-1; i++) {
-        _protected_input.psi_mm[i] = demands_array.data[i];
+    for(int i=0; i<N_CHANNELS; i++) {
+        _protected_input.psi_mm[i] = MAX_ACTUATOR_LIMIT_MM*((double)demands_array.data[i]/511);
+        _protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
     }
-    short int demands_array_length = sizeof(demands_array.data)/sizeof(demands_array.data[0]);
-    _protected_input.speed_mmps = demands_array.data[demands_array_length-1];*/
-    _protected_input.psi_mm[0] = demands_array.data[0];
-    _protected_input.psi_mm[1] = demands_array.data[1];
-    _protected_input.psi_mm[2] = demands_array.data[2];
-    _protected_input.psi_mm[3] = demands_array.data[3];
-    _protected_input.psi_mm[4] = demands_array.data[4];
-    _protected_input.psi_mm[5] = demands_array.data[5];
-    _protected_input.psi_mm[6] = demands_array.data[6];
-    _protected_input.psi_mm[7] = demands_array.data[7];
-    _protected_input.psi_mm[8] = demands_array.data[8];
-    _protected_input.speed_mmps = demands_array.data[9];
+    //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    //short int demands_array_length = sizeof(demands_array.data)/sizeof(demands_array.data[0]);
+    //_protected_input.speed_mmps = demands_array.data[demands_array_length-1];
+    _protected_input.speed_mmps = MAX_SPEED_MMPS*((double)demands_array.data[9]/511);
+    _protected_input.speed_mmps = min( max(_protected_input.speed_mmps,0.0) , (double)MAX_SPEED_MMPS );
     recv_mutex.unlock();
     _input = _protected_input;
+    /*char array[20];
+    sprintf(array, "Speed: %f", _input.speed_mmps);
+    send_text_message(array);
+    sprintf(array, "Psi[0]: %f", _input.psi_mm[0]);
+    send_text_message(array);
+    sprintf(array, "Psi[1]: %f", _input.psi_mm[1]);
+    send_text_message(array);*/
     newData.release();
 }
 
@@ -83,12 +87,17 @@
 
 // OUTPUT
 
+void HLComms::send_text_message(char text[]) {
+    _text_msg.data = text;
+    _text_pub->publish(&_text_msg);
+}
+
 void HLComms::send_duration_message(double dblTime) {
     _duration_msg.data = dblTime;
     _duration_pub->publish(&_duration_msg);
 }
 
-void HLComms::send_sensor_message(double positions[], double pressures[]) {
+void HLComms::send_sensor_message(unsigned int positions[], unsigned int pressures[]) {
     short int i_channel;
     for(short int i=0; i<N_CHANNELS*2; i++) {
         i_channel = i%N_CHANNELS;