3 #include "roboint/Output.h"
4 #include "roboint/Motor.h"
5 #include "roboint/Inputs.h"
8 FT_TRANSFER_AREA *transfer_area = NULL;
11 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
12 if (msg->speed == 0) {
13 transfer_area->M_Main &= ~(1<<(msg->num-1));
15 transfer_area->M_Main |= (1<<(msg->num-1));
17 transfer_area->MPWM_Main[msg->num-1] = msg->speed;
21 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
22 unsigned char iDirection = 0;
24 if (msg->speed > 0) iDirection = 0x1;
25 else if (msg->speed < 0) iDirection = 0x2;
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;
34 int main(int argc, char **argv)
40 // Get List of USB devices
41 InitFtUsbDeviceList();
43 // Get handle on a device
44 if ((hFt = GetFtUsbDeviceHandle(0)) == NULL) {
45 fprintf(stderr, "No ft Device found\n");
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);
53 StartFtTransferArea(hFt, NULL);
54 transfer_area = GetFtTransferAreaAddress(hFt);
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.
63 * You must call one of the versions of ros::init() before using any other
64 * part of the ROS system.
66 ros::init(argc, argv, "libft_adapter");
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.
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);
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.
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.
95 ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 1000);
96 ros::Rate loop_rate(5);
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);
105 msg.d1 = transfer_area->D1;
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.
113 chatter_pub.publish(msg);
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.
125 StopFtTransferArea(hFt);