Diff: HLComms.cpp

Revision:

30:6c8eea90735e

Parent:

29:10a5cf37a875

Child:

31:08cb04eb75fc

--- a/HLComms.cpp	Fri Feb 08 18:36:15 2019 +0000
+++ b/HLComms.cpp	Fri Feb 15 10:29:23 2019 +0000
@@ -26,7 +26,8 @@
     _nh.initNode();
     wait_ms(1000);
     
-    ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
+    //ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
+    ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
     _nh.subscribe(demands_sub);
     
     _text_pub = new ros::Publisher("ML_TextOut", &_text_msg);
@@ -57,27 +58,45 @@
 
 // INPUT
 
-void HLComms::receive_demands(const std_msgs::Int16MultiArray &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; 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 );
+        _protected_input.psi_mm[i] = ((MAX_ACTUATOR_LIMIT_MM+1.0)*((double)demands_array.data[i]/65535)-1.0);
+        if( _protected_input.psi_mm[i]<-0.5 ) {
+            _protected_input.psi_mm[i] = -1.0;
+        } else {
+            _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.speed_mmps = MAX_SPEED_MMPS*((double)demands_array.data[9]/511);
+    _protected_input.speed_mmps = MAX_SPEED_MMPS*((double)demands_array.data[9]/65535);
     _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);*/
+    //char array[20];
+    //sprintf(array, "Speed: %f", _input.speed_mmps);
+    //send_text_message(array);
+    newData.release();
+}*/
+
+void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) {
+    struct demands_struct _protected_input;
+    recv_mutex.lock();
+    for(int i=0; i<N_CHANNELS; i++) {
+        _protected_input.psi_mm[i] = (double)demands_array.data[i];
+        if( _protected_input.psi_mm[i]<-0.5 ) {
+            _protected_input.psi_mm[i] = -1.0;
+        } else {
+            _protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
+        }
+    }
+    _protected_input.speed_mmps = (double)demands_array.data[9];
+    _protected_input.speed_mmps = min( max(_protected_input.speed_mmps,0.0) , (double)MAX_SPEED_MMPS );
+    recv_mutex.unlock();
+    _input = _protected_input;
     newData.release();
 }