lost sync with device ...restarting...
#include <ros/ros.h>
#include <std_msgs/UInt16.h>
#include <stdio.h>
#define PI 3.1415926
float d1,d2,d3,d4,theta1,theta2,Theta1,Theta2;
int h=30;
void servoes(){
theta1=PI/2;
theta2=PI/2;
d1=105.0;
d2=130.0;
d3=d1*cos(theta1-PI/4)+d2*cos(theta2-theta1+PI/4);
d4=d3-h;
Theta2=acos((pow(d1,2)+pow(d2,2)-pow(d3,2))/(2*d1*d2))-acos((pow(d1,2)+pow(d2,2)-pow(d4,2))/(2*d1*d2))+theta2;
Theta1=atan(d1*sin(Theta2)/(d1+d2*cos(Theta2)));
}
int main(int argc, char **argv) {
ros::init(argc, argv, "servo_pub");
ros::NodeHandle n;
ros::Publisher my_publisher_object = n.advertise<std_msgs::UInt16>("servo", 1);
std_msgs::UInt16 msg;
servoes();
int theta3=(Theta1+PI/4)/PI*2000+500;
int theta4=Theta2/PI*2000+500;
ros::Rate loop_rate(10);
while (ros::ok())
{
msg.data =theta3;
my_publisher_object.publish(msg);
loop_rate.sleep();
}
}
here are my code and here are my arduino code
enter code here
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <Servo.h>
#include<stdio.h>
#include <ros.h>
#include <std_msgs/UInt16.h>
#include<std_msgs/String.h>
int h;
ros::NodeHandle nh;
void servo_cb( const std_msgs::UInt16& cmd_msg){
h=cmd_msg.data;
}
ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);
void setup(){
Serial.begin(115200);
nh.initNode();
nh.subscribe(sub);
}
void loop(){
if(Serial.available()>0)
{
Serial.print("#0 P");
Serial.print(h);
Serial.println(" T500");
delay(100);
}
nh.spinOnce();
nh.spinOnce();
nh.spinOnce();
}
and i run my code,but it tell me : lost sync with device ...restarting...; again and again.I need help please...
Can you describe how you are starting these nodes?
$roscore $rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200 $rosrun servo servo