Merge branch 'devel' into 'master'

Devel See merge request MAB_Robotics/md80_usbcan!29
mabrobotics · Jan 19, 2023 · cc6cb13 · cc6cb13
2 parents 687300b + ad16e47
commit cc6cb13
Showing 1 changed file with 16 additions and 11 deletions.
diff --git a/src/md80.cpp b/src/md80.cpp
@@ -31,7 +31,6 @@ void Md80::setPositionControllerParams(float kp, float ki, float kd, float iWind
 }
 void Md80::setVelocityControllerParams(float kp, float ki, float kd, float iWindup)
 {
-	regulatorsAdjusted = true;
 	velocityRegulatorAdjusted = true;
 	velocityController.kp = kp;
 	velocityController.ki = ki;
@@ -55,33 +54,39 @@ void Md80::__updateCommandFrame()
 			break;
 		case Md80Mode_E::IMPEDANCE:
 			if (regulatorsAdjusted)
+			{
 				packImpedanceFrame();
+				regulatorsAdjusted = false;
+			}
 			else
 				packMotionTargetsFrame();
 			break;
 		case Md80Mode_E::POSITION_PID:
 			if (regulatorsAdjusted)
-				if (velocityRegulatorAdjusted)
-				{
-					velocityRegulatorAdjusted = false;
-					packVelocityFrame();
-				}
-				else
-					packPositionFrame();
+			{
+				packPositionFrame();
+				regulatorsAdjusted = false;
+			}
+			else if (velocityRegulatorAdjusted)
+			{
+				packVelocityFrame();
+				velocityRegulatorAdjusted = false;
+			}
 			else
 				packMotionTargetsFrame();
 			break;
 		case Md80Mode_E::VELOCITY_PID:
-			if (regulatorsAdjusted)
+			if (velocityRegulatorAdjusted)
+			{
 				packVelocityFrame();
+				velocityRegulatorAdjusted = false;
+			}
 			else
 				packMotionTargetsFrame();
 			break;
 		default:
 			break;
 	}
-	/* send updated gains only when modified */
-	regulatorsAdjusted = false;
 }
 void Md80::__updateResponseData(StdMd80ResponseFrame_t* _responseFrame)
 {