From faff49b40e45239ec256cef8f94f61fc8120a4a4 Mon Sep 17 00:00:00 2001
From: Piotr <szlachcic49er@gmail.com>
Date: Tue, 30 Apr 2019 11:25:35 +0200
Subject: [PATCH] fix angleCallback

---
 main.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/main.cpp b/main.cpp
index d193328..5848de0 100644
--- a/main.cpp
+++ b/main.cpp
@@ -31,6 +31,7 @@ DiffController *dc;
 class ServoWrapper
 {
 	int num;
+	int per;
 	IServo& servo;
 
 public:
@@ -40,13 +41,18 @@ class ServoWrapper
 
 	void angleCallback(const std_msgs::Int16& msg)
 	{
-		servo.setPeriod(SERVO_PERIOD);
+		if (per!=SERVO_PERIOD)
+		{
+			servo.setPeriod(SERVO_PERIOD);
+			per=SERVO_PERIOD;
+		}
 		servo.rotAbs(msg.data);
 		Serial.printf("[servo%dAngleCallback] angle: %d\r\n", num, msg.data);
 	}
 
 	void pwmCallback(const std_msgs::UInt16MultiArray& msg)
 	{
+		per=msg.data[0];
 		servo.setPeriod(msg.data[0]);
 		servo.setWidth(msg.data[1]);
 		Serial.printf("[servo%dPWMCallback] period: %d width: %d\r\n", num, msg.data[0], msg.data[1]);