src/libft_adapter.cpp

   1 #include <stdlib.h>
   2 #include "roboint.h"
   3 #include "ros/ros.h"
   4 #include "roboint/Output.h"
   5 #include "roboint/Motor.h"
   6 #include "roboint/Inputs.h"
   7
   8
   9 FT_TRANSFER_AREA *transfer_area = NULL;
  10
  11
  12 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
  13         if (msg->speed == 0) {
  14                 transfer_area->M_Main &= ~(1<<(msg->num-1));
  15         } else {
  16                 transfer_area->M_Main |= (1<<(msg->num-1));
  17         }
  18         transfer_area->MPWM_Main[msg->num-1] = msg->speed;
  19 }
  20
  21
  22 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
  23         unsigned char iDirection = 0;
  24         int speed = abs(msg->speed);
  25
  26         if (msg->speed > 0) iDirection = 0x1;
  27         else if (msg->speed < 0) iDirection = 0x2;
  28
  29         transfer_area->M_Main &= ~(3<<(msg->num-1)*2);
  30         transfer_area->M_Main |= iDirection<<(msg->num-1)*2;
  31         transfer_area->MPWM_Main[(msg->num-1)*2] = speed;
  32         transfer_area->MPWM_Main[(msg->num-1)*2 + 1] = speed;
  33 }
  34
  35
  36 int main(int argc, char **argv)
  37 {
  38         FT_HANDLE hFt = NULL;
  39
  40         InitFtLib();
  41
  42         // Get List of USB devices
  43         InitFtUsbDeviceList();
  44
  45         // Get handle on a device
  46         if ((hFt = GetFtUsbDeviceHandle(0)) == NULL) {
  47                 fprintf(stderr, "No ft Device found\n");
  48                 return 1;
  49         }
  50
  51         // Open connection
  52         OpenFtUsbDevice(hFt);
  53         SetFtDistanceSensorMode(hFt, IF_DS_INPUT_DISTANCE, IF_DS_INPUT_TOL_STD, IF_DS_INPUT_TOL_STD, 100, 100, IF_DS_INPUT_REP_STD, IF_DS_INPUT_REP_STD);
  54
  55         StartFtTransferArea(hFt, NULL);
  56         transfer_area = GetFtTransferAreaAddress(hFt);
  57
  58         /**
  59          * The ros::init() function needs to see argc and argv so that it can perform
  60          * any ROS arguments and name remapping that were provided at the command line. For programmatic
  61          * remappings you can use a different version of init() which takes remappings
  62          * directly, but for most command-line programs, passing argc and argv is the easiest
  63          * way to do it.  The third argument to init() is the name of the node.
  64          *
  65          * You must call one of the versions of ros::init() before using any other
  66          * part of the ROS system.
  67          */
  68         ros::init(argc, argv, "libft_adapter");
  69
  70         /**
  71          * NodeHandle is the main access point to communications with the ROS system.
  72          * The first NodeHandle constructed will fully initialize this node, and the last
  73          * NodeHandle destructed will close down the node.
  74          */
  75         ros::NodeHandle n;
  76
  77         ros::Subscriber sub_set_output = n.subscribe("ft/set_output", 8, cb_set_output);
  78         ros::Subscriber sub_set_motor = n.subscribe("ft/set_motor", 4, cb_set_motor);
  79
  80         /**
  81          * The advertise() function is how you tell ROS that you want to
  82          * publish on a given topic name. This invokes a call to the ROS
  83          * master node, which keeps a registry of who is publishing and who
  84          * is subscribing. After this advertise() call is made, the master
  85          * node will notify anyone who is trying to subscribe to this topic name,
  86          * and they will in turn negotiate a peer-to-peer connection with this
  87          * node.  advertise() returns a Publisher object which allows you to
  88          * publish messages on that topic through a call to publish().  Once
  89          * all copies of the returned Publisher object are destroyed, the topic
  90          * will be automatically unadvertised.
  91          *
  92          * The second parameter to advertise() is the size of the message queue
  93          * used for publishing messages.  If messages are published more quickly
  94          * than we can send them, the number here specifies how many messages to
  95          * buffer up before throwing some away.
  96          */
  97         ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
  98         ros::Rate loop_rate(100);
  99
 100         while(ros::ok()) {
 101                 roboint::Inputs msg;
 102
 103                 msg.input[0] = 0; // unused, Hardware starts at 1
 104                 for (int i=1; i<=8; i++) {
 105                         msg.input[i] = (transfer_area->E_Main & (1<<(i-1))) >> (i-1);
 106                 }
 107                 msg.d1 = transfer_area->D1;
 108
 109                 /**
 110                  * The publish() function is how you send messages. The parameter
 111                  * is the message object. The type of this object must agree with the type
 112                  * given as a template parameter to the advertise<>() call, as was done
 113                  * in the constructor above.
 114                  */
 115                 chatter_pub.publish(msg);
 116
 117                 /**
 118                  * ros::spin() will enter a loop, pumping callbacks.  With this version, all
 119                  * callbacks will be called from within this thread (the main one).  ros::spin()
 120                  * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
 121                  */
 122                 ros::spinOnce();
 123
 124                 loop_rate.sleep();
 125         }
 126
 127         StopFtTransferArea(hFt);
 128
 129         // Close connection
 130         CloseFtDevice(hFt);
 131
 132         CloseFtLib();
 133
 134         return 0;
 135 }
 136