rename chatter_pub to pub_inputs

[ros_roboint.git] / src / libft_adapter.cpp

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index 9247894934281b491fd50bbc424125a765c32d1e..925dfd54ea37dd444717e126563e283ac6420850 100644 (file)
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -1,3 +1,5 @@
+#include <stdlib.h>
+#include <pthread.h>
 #include "roboint.h"
 #include "ros/ros.h"
 #include "roboint/Output.h"
@@ -5,29 +7,28 @@
 #include "roboint/Inputs.h"
 
 
-FT_TRANSFER_AREA *transfer_area = NULL;
-
+static FT_TRANSFER_AREA *transfer_area = NULL;
+static char pwm[8] = {0};
+static char pwm_next[8] = {0};
+static pthread_mutex_t pwm_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
-       if (msg->speed == 0) {
-               transfer_area->M_Main &= ~(1<<(msg->num-1));
-       } else {
-               transfer_area->M_Main |= (1<<(msg->num-1));
-       }
-       transfer_area->MPWM_Main[msg->num-1] = msg->speed;
+       pthread_mutex_lock(&pwm_mutex);
+       pwm_next[msg->num] = abs(msg->speed);
+       pthread_mutex_unlock(&pwm_mutex);
 }
 
 
 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
-       unsigned char iDirection = 0;
-
-       if (msg->speed > 0) iDirection = 0x1;
-       else if (msg->speed < 0) iDirection = 0x2;
-
-       transfer_area->M_Main &= ~(3<<(msg->num-1)*2);
-       transfer_area->M_Main |= iDirection<<(msg->num-1)*2;
-       transfer_area->MPWM_Main[(msg->num-1)*2] = msg->speed;
-       transfer_area->MPWM_Main[(msg->num-1)*2 + 1] = msg->speed;
+       pthread_mutex_lock(&pwm_mutex);
+       if (msg->speed > 0) {
+               pwm_next[msg->num*2] = abs(msg->speed);
+               pwm_next[msg->num*2+1] = 0;
+       } else {
+               pwm_next[msg->num*2] = 0;
+               pwm_next[msg->num*2+1] = abs(msg->speed);
+       }
+       pthread_mutex_unlock(&pwm_mutex);
 }
 
 
@@ -92,17 +93,38 @@ int main(int argc, char **argv)
         * than we can send them, the number here specifies how many messages to
         * buffer up before throwing some away.
         */
-       ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 1000);
-       ros::Rate loop_rate(5);
+       ros::Publisher pub_inputs = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
+       ros::Rate loop_rate(100);
 
        while(ros::ok()) {
                roboint::Inputs msg;
 
-               msg.input[0] = 0; // unused, Hardware starts at 1
-               for (int i=1; i<=8; i++) {
-                       msg.input[i] = (transfer_area->E_Main & (1<<(i-1))) >> (i-1);
+               pthread_mutex_lock(&pwm_mutex);
+               msg.header.stamp = ros::Time::now();
+               sem_wait(&hFt->lock);
+               for (int i=0; i<=7; i++) {
+                       msg.input[i] = (transfer_area->E_Main & (1<<i)) >> i;
+               }
+               for (int i=0; i<=7; i++) {
+                       msg.output[i] = pwm[i];
+                       pwm[i] = pwm_next[i];
+
+                       if (pwm[i] == 0) {
+                               transfer_area->M_Main &= ~(1<<(i));
+                       } else {
+                               transfer_area->M_Main |= (1<<(i));
+                       }
+                       transfer_area->MPWM_Main[i] = pwm[i];
                }
+               msg.ax = transfer_area->AX;
+               msg.ay = transfer_area->AY;
+               msg.a1 = transfer_area->A1;
+               msg.a2 = transfer_area->A2;
+               msg.av = transfer_area->AV;
                msg.d1 = transfer_area->D1;
+               msg.d2 = transfer_area->D2;
+               sem_post(&hFt->lock);
+               pthread_mutex_unlock(&pwm_mutex);
 
                /**
                 * The publish() function is how you send messages. The parameter
@@ -110,7 +132,7 @@ int main(int argc, char **argv)
                 * given as a template parameter to the advertise<>() call, as was done
                 * in the constructor above.
                 */
-               chatter_pub.publish(msg);
+               pub_inputs.publish(msg);
 
                /**
                 * ros::spin() will enter a loop, pumping callbacks.  With this version, all