X-Git-Url: https://defiant.homedns.org/gitweb/?a=blobdiff_plain;f=src%2Flibft_adapter.cpp;h=24fa1c474f420f7fce679338a5036d5a4c7d9ca0;hb=839da311672219243b7f0bc3291b331f92a395c8;hp=27936b7f4fcb8aa90760dc87d6529b29767e7fa5;hpb=7ed26bb9678da05ebd73e6299b90908760a04780;p=ros_roboint.git

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index 27936b7..24fa1c4 100644
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -1,4 +1,5 @@
 #include <stdlib.h>
+#include <pthread.h>
 #include "roboint.h"
 #include "ros/ros.h"
 #include "roboint/Output.h"
@@ -6,30 +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;
-	int speed = abs(msg->speed);
-
-	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] = speed;
-	transfer_area->MPWM_Main[(msg->num-1)*2 + 1] = 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);
 }
 
 
@@ -100,11 +99,31 @@ int main(int argc, char **argv)
 	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);
+		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