void DoSpeedProfile() {
if (bWheelMotorEnable)
{
DoMoveCommand(); // speed profile
UpdateCurSpeed(); // update curSpeed[] according to targetSpeed[]
UpdateWheelPos();
UpdateRobotPos(); // To update the robot's current position based on the robot
}
else
ResetSpeedProfileData();
}
void DoSpeedProfile() {
if (bMotorEnableFlag) {
DoMoveCommand(); // execute speed profile
UpdateCurSpeed(); // update current speed according to target speed
UpdateWheelPos(); // To update robot's current position (for use with speed profile)
// UpdateRobotPos(); // To update the robot's current position
// if (curSpeed[1] || curSpeed[0]) {
// LogDataFn(debugData0, debugData1, curSpeed[0], curSpeed[1]);
// LogDataFn(curSpeed[0], curSpeed[1], curTargetPos[0], finalPos[0]);
// }
} else {
ResetSpeedProfileData();
}
}
