From 1fad18a34b96fb8a18c0d6516ec885dd712c5c3a Mon Sep 17 00:00:00 2001 From: Erik Andresen 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 +#include #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; } + 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.2