From 1fad18a34b96fb8a18c0d6516ec885dd712c5c3a Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Thu, 5 Sep 2013 21:49:21 +0200
Subject: [PATCH] include outputs in input message
---
msg/Inputs.msg | 1 +
scripts/robo_explorer.py | 18 +++++++--------
src/libft_adapter.cpp | 48 +++++++++++++++++++++++++---------------
3 files changed, 39 insertions(+), 28 deletions(-)
diff --git a/msg/Inputs.msg b/msg/Inputs.msg
index fb54170..d525cfa 100644
--- a/msg/Inputs.msg
+++ b/msg/Inputs.msg
@@ -1,4 +1,5 @@
bool[8] input
+int8[8] output
int16 ax
int16 ay
int16 a1
diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index 24584e7..514fe6d 100755
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -16,7 +16,6 @@ class RoboExplorer:
def __init__(self):
rospy.init_node('robo_explorer')
- self.speed = (0, 0)
self.x = 0
self.y = 0
self.alpha = 0
@@ -62,14 +61,15 @@ class RoboExplorer:
in_now = msg.input[:2]
if self.last_in is not None:
in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
+ print "update", in_diff, msg.output
# fix in_diff from actual motor direction
- if self.speed[0] < 0:
+ if msg.output[1] > 0: # left reverse
in_diff[0] = -in_diff[0]
- elif self.speed[0] == 0:
+ elif msg.output[0] == 0 and msg.output[1] == 0: # left stop
in_diff[0] = 0
- if self.speed[1] < 0:
+ if msg.output[3] > 0: # right reverse
in_diff[1] = -in_diff[1]
- elif self.speed[1] == 0:
+ elif msg.output[2] == 0 and msg.output[3] == 0: # right stop
in_diff[1] = 0
dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8 # steps_changed in different direction => m
@@ -180,19 +180,17 @@ class RoboExplorer:
if speed_r < -7: speed_r = -7
elif speed_r > 7: speed_r = 7
- #print "Speed wanted: %.2f m/s %.2f rad/s, set: %d %d" % (trans, rot, round(speed_l), round(speed_r))
+ print "Speed wanted: %.2f m/s %.2f rad/s, set: %d %d" % (trans, rot, round(speed_l), round(speed_r))
outmsg = Motor()
- outmsg.num = 1
+ outmsg.num = 0
outmsg.speed = round(speed_l)
self.pub_motor.publish(outmsg)
outmsg = Motor()
- outmsg.num = 2
+ outmsg.num = 1
outmsg.speed = round(speed_r)
self.pub_motor.publish(outmsg)
- self.speed = (speed_l, speed_r)
-
if __name__ == '__main__':
RoboExplorer()
diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index 588f11e..bacd2d7 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] = 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] = msg->speed;
+ pwm_next[msg->num*2+1] = 0;
+ } else {
+ pwm_next[msg->num*2] = 0;
+ pwm_next[msg->num*2+1] = msg->speed;
+ }
+ pthread_mutex_unlock(&pwm_mutex);
}
@@ -100,10 +99,22 @@ int main(int argc, char **argv)
while(ros::ok()) {
roboint::Inputs msg;
+ 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] = abs(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] = abs(pwm[i]);
+ }
msg.ax = transfer_area->AX;
msg.ay = transfer_area->AY;
msg.a1 = transfer_area->A1;
@@ -112,6 +123,7 @@ int main(int argc, char **argv)
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
--
2.39.5