X-Git-Url: https://defiant.homedns.org/gitweb/?a=blobdiff_plain;f=src%2Flibft_adapter.cpp;h=63d710dcd063a1bd29d89d88d91a8cdb5aa99e15;hb=e2df79c17569d45bd959aec083c5fd6e72ca960b;hp=bacd2d7f79f033853b8f7e7faf38594c6e901426;hpb=1fad18a34b96fb8a18c0d6516ec885dd712c5c3a;p=ros_roboint.git

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index bacd2d7..63d710d 100644
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -8,13 +8,13 @@
 
 
 static FT_TRANSFER_AREA *transfer_area = NULL;
-static char pwm[8] = {0};
-static char pwm_next[8] = {0};
+static int8_t pwm[8] = {0};
+static int8_t pwm_next[8] = {0};
 static pthread_mutex_t pwm_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
 	pthread_mutex_lock(&pwm_mutex);
-	pwm_next[msg->num] = msg->speed;
+	pwm_next[msg->num] = abs(msg->speed);
 	pthread_mutex_unlock(&pwm_mutex);
 }
 
@@ -22,11 +22,11 @@ void cb_set_output(const ::roboint::OutputConstPtr& msg) {
 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
 	pthread_mutex_lock(&pwm_mutex);
 	if (msg->speed > 0) {
-		pwm_next[msg->num*2] = msg->speed;
+		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] = msg->speed;
+		pwm_next[msg->num*2+1] = abs(msg->speed);
 	}
 	pthread_mutex_unlock(&pwm_mutex);
 }
@@ -93,19 +93,20 @@ 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", 10);
+	ros::Publisher pub_inputs = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
 	ros::Rate loop_rate(100);
 
 	while(ros::ok()) {
 		roboint::Inputs msg;
 
 		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] = abs(pwm[i]);
+			msg.output[i] = pwm[i];
 			pwm[i] = pwm_next[i];
 
 			if (pwm[i] == 0) {
@@ -113,7 +114,7 @@ int main(int argc, char **argv)
 			} else {
 				transfer_area->M_Main |= (1<<(i));
 			}
-			transfer_area->MPWM_Main[i] = abs(pwm[i]);
+			transfer_area->MPWM_Main[i] = pwm[i];
 		}
 		msg.ax = transfer_area->AX;
 		msg.ay = transfer_area->AY;
@@ -131,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