X-Git-Url: https://defiant.homedns.org/gitweb/?a=blobdiff_plain;f=src%2Flibft_adapter.cpp;h=63d710dcd063a1bd29d89d88d91a8cdb5aa99e15;hb=e2df79c17569d45bd959aec083c5fd6e72ca960b;hp=9247894934281b491fd50bbc424125a765c32d1e;hpb=e3560f756faac781896e0402caca83d330eee1a4;p=ros_roboint.git diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp index 9247894..63d710d 100644 --- a/src/libft_adapter.cpp +++ b/src/libft_adapter.cpp @@ -1,3 +1,5 @@ +#include +#include #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 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) { - 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("ft/get_inputs", 1000); - ros::Rate loop_rate(5); + ros::Publisher pub_inputs = n.advertise("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; + } + 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