50 #include <hitec5980sg/hitec5980sg.h>
51 #include <geometry_msgs/Point.h>
65 servos(device.c_str())
72 if ((point.z <= 0) && (point.y <= 0))
82 if ((point.z >= 0) && (point.z <= 255) && (point.y >= 0) && (point.y <= 255))
86 servos.SetPosition(53,1600);
87 x_before = servos.SetSpeedPosition(53,point.y);
89 servos.SetPosition(41,2000);
90 y_before = servos.SetSpeedPosition(41,point.z);
94 servos.SetPosition(53,1600);
95 x_before = servos.SetSpeedPosition(53,point.y);
97 servos.SetPosition(41,600);
98 y_before = servos.SetSpeedPosition(41,point.z);
102 servos.SetPosition(53,2000);
103 x_before = servos.SetSpeedPosition(53,point.y);
105 servos.SetPosition(41,2000);
106 y_before = servos.SetSpeedPosition(41,point.z);
110 servos.SetPosition(53,2000);
111 x_before = servos.SetSpeedPosition(53,point.y);
113 servos.SetPosition(41,600);
114 y_before = servos.SetSpeedPosition(41,point.z);
119 std::cout <<
"invalid velocity: " << point.z << std::endl;
127 int main(
int argc,
char** argv)
129 ros::init( argc, argv,
"head_motion" );
131 ros::NodeHandle n(
"~");
133 Head head(
"/dev/ttyUSB0");
134 std::cout <<
"initializing head motion..." << std::endl;
short unsigned int x_before
void positionHandler(const geometry_msgs::Point &point)
short unsigned int y_before
int main(int argc, char **argv)