     return q;
 }
 
-
-
-
-
-void setup()
-{
-    nh.initNode();
-    broadcaster.init(nh);
-
-    nh.advertise(rangePublisher1);
-    nh.advertise(rangePublisher2);
-}
-
-float tempNumber = 0;
+double RadianToDegree(double rad)
+{
+    return (rad * 4068) / 71.0;
+}
+
+double DegreeToRadian(double deg)
+{
+    return (deg * 71) / 4068.0;
+}
+
+
+// ------------------------------
+
+
+
+long GetRange(int id)
+{
+    long range = 0;
+    if(id == 1)
+    {
+        range = sonar1.ping_cm();
+    }
+
+    if(id == 2)
+    {
+        range = sonar2.ping_cm();
+    }
+
+    return range;
+}
+
+
+uint8_t GetNextServoAngle()
+{
+    if(servoPos <= 0)
+    {
+        servoMovement = right;
+        servoPos = 0;
+    }
+
+    if(servoPos >= 180)
+    {
+        servoMovement = left;
+        servoPos = 180;
+    }
+
+
+    if(servoMovement == right)
+    {
+        servoPos += servoRotationAngel;
+    }
+    else
+    {
+        servoPos -= servoRotationAngel;
+    }
+
+    return servoPos;
+}
+
+
+void SendServoAngleToROS(int angle)
+{
+
+}
+
+void SendTransformToROS()
+{
+
+}
+
+
+void SendDistanceToROS(int sensorID, float rangeInCentimeter)
+{
+    rangeMessage.range = rangeInCentimeter/100;
+    rangeMessage.header.stamp = nh.now();
+
+    if(sensorID == 1)
+    {
+        rangeMessage.header.frame_id = servo_node_1;
+        rangePublisher1.publish(&rangeMessage);
+    }
+
+    if(sensorID == 2)
+    {
+        rangeMessage.header.frame_id = servo_node_2;
+        rangePublisher2.publish(&rangeMessage);
+    }
+}
+
 
 void loop()
 {
-    tempNumber += 0.1;
+    long rangeFront = GetRange(1);
+    long rangeBack = GetRange(2);
+
+    uint8_t actualServoPos = ServoMotor.read();
+    if(servoPos == actualServoPos)
+    {
+        ServoMotor.write(GetNextServoAngle());
+        if(!nh.connected())
+        {
+            delay(5);
+        }
+    }
+
+    if(rangeFront > 0)
+    {
+        SendDistanceToROS(1, rangeFront);
+    }
+
+    if(rangeBack > 0)
+    {
+        SendDistanceToROS(2, rangeBack);
+    }
+
+    SendServoAngleToROS(servoPos);
+
+
+
+
+
+
+
 
     t.header.frame_id = odom;
     t.child_frame_id = base_link;

char servo_node_2[] = "/servo_node_2";
char base_link[] = "/base_link";
char odom[] = "/odom";


static geometry_msgs::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw) {

    broadcaster.sendTransform(t);




    nh.spinOnce();
}
