Merge pull request #1 from szlachcic/master

fix angleCallback
LeoRover · May 10, 2019 · 750270a · 750270a
2 parents 9639c1f + faff49b
commit 750270a
Showing 1 changed file with 7 additions and 1 deletion.
diff --git 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]);