Only update position array when we are in synched mode (otherwise this variable is not needed).

aentinger · aentinger · commit 7acbf71a0499 · 2022-08-24T08:22:34.000+02:00
diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp
@@ -236,13 +236,15 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle)
     Serial.println(msg);
   }
 
-  _targetPosition[idToArrayIndex(id)] = target_position_deg;
-
   if (_positionMode == PositionMode::IMMEDIATE)
   {
     mbed::ScopedLock<rtos::Mutex> lock(_mtx);
     writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), target_position_deg);
   }
+  else if (_positionMode == PositionMode::SYNC)
+  {
+    _targetPosition[idToArrayIndex(id)] = target_position_deg;
+  }
 }
 
 float SmartServoClass::getPosition(uint8_t const id)

Original file line number	Diff line number	Diff line change
`@@ -236,13 +236,15 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle)`
`236`	`236`	`Serial.println(msg);`
`237`	`237`	`}`
`238`	`238`
`239`		`- _targetPosition[idToArrayIndex(id)] = target_position_deg;`
`240`		`-`
`241`	`239`	`if (_positionMode == PositionMode::IMMEDIATE)`
`242`	`240`	`{`
`243`	`241`	`mbed::ScopedLock<rtos::Mutex> lock(_mtx);`
`244`	`242`	`writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), target_position_deg);`
`245`	`243`	`}`
	`244`	`+ else if (_positionMode == PositionMode::SYNC)`
	`245`	`+ {`
	`246`	`+ _targetPosition[idToArrayIndex(id)] = target_position_deg;`
	`247`	`+ }`
`246`	`248`	`}`
`247`	`249`
`248`	`250`	`float SmartServoClass::getPosition(uint8_t const id)`