4 #include "roboint/Output.h"
5 #include "roboint/Motor.h"
6 #include "roboint/Inputs.h"
9 FT_TRANSFER_AREA *transfer_area = NULL;
12 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
13 if (msg->speed == 0) {
14 transfer_area->M_Main &= ~(1<<(msg->num-1));
16 transfer_area->M_Main |= (1<<(msg->num-1));
18 transfer_area->MPWM_Main[msg->num-1] = msg->speed;
22 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
23 unsigned char iDirection = 0;
24 int speed = abs(msg->speed);
26 if (msg->speed > 0) iDirection = 0x1;
27 else if (msg->speed < 0) iDirection = 0x2;
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;
36 int main(int argc, char **argv)
42 // Get List of USB devices
43 InitFtUsbDeviceList();
45 // Get handle on a device
46 if ((hFt = GetFtUsbDeviceHandle(0)) == NULL) {
47 fprintf(stderr, "No ft Device found\n");
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);
55 StartFtTransferArea(hFt, NULL);
56 transfer_area = GetFtTransferAreaAddress(hFt);
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.
65 * You must call one of the versions of ros::init() before using any other
66 * part of the ROS system.
68 ros::init(argc, argv, "libft_adapter");
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.
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);
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.
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.
97 ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
98 ros::Rate loop_rate(100);
103 sem_wait(&hFt->lock);
104 for (int i=0; i<=7; i++) {
105 msg.input[i] = (transfer_area->E_Main & (1<<i)) >> i;
107 msg.ax = transfer_area->AX;
108 msg.ay = transfer_area->AY;
109 msg.a1 = transfer_area->A1;
110 msg.a2 = transfer_area->A2;
111 msg.av = transfer_area->AV;
112 msg.d1 = transfer_area->D1;
113 msg.d2 = transfer_area->D2;
114 sem_post(&hFt->lock);
117 * The publish() function is how you send messages. The parameter
118 * is the message object. The type of this object must agree with the type
119 * given as a template parameter to the advertise<>() call, as was done
120 * in the constructor above.
122 chatter_pub.publish(msg);
125 * ros::spin() will enter a loop, pumping callbacks. With this version, all
126 * callbacks will be called from within this thread (the main one). ros::spin()
127 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
134 StopFtTransferArea(hFt);