src/libft_adapter.cpp

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