private void UpdateCurrentPosition(ALMotionProxy motionProxy) { float[] positions = motionProxy.GetRobotPosition(true); currentPositionX = positions[0]; currentPositionZ = positions[1]; currentTheta = positions[2]; }
private void ThreadedLoop() { ALMotionProxy motionProxy = new ALMotionProxy(address, port); if (!motionProxy.IsConnected()) { Debug.Log("Unable to connect to robot"); return; } motionProxy.MoveInit(); while (isRunning) { if (isUpdating) { motionProxy.SetAngles(ALL_JOINTS, new float[] { headPitch, headYaw, leftShoulderPitch, leftShoulderRoll, leftElbowYaw, leftElbowRoll, rightShoulderPitch, rightShoulderRoll, rightElbowYaw, rightElbowRoll, RightWristYaw, LeftWristYaw, RightHandClosedAmount, LeftHandClosedAmount }, SPEED_FRACTION); UpdateCurrentPosition(motionProxy); float thetaDiff = CalculateThetaDiff(); float xDiff = DesiredPositionX - currentPositionX; float zDiff = DesiredPositionZ - currentPositionZ; if (Math.Abs(thetaDiff) < ROTATION_THRESHOLD) { thetaDiff = 0; } if (Math.Abs(xDiff) < MOVEMENT_THRESHOLD) { xDiff = 0; } if (Math.Abs(zDiff) < MOVEMENT_THRESHOLD) { zDiff = 0; } xDiff = 0; zDiff = 0; motionProxy.MoveToAsync(x, y, thetaDiff); } else { motionProxy.SetAngles(ALL_JOINTS, DEFAULT_JOINT_VALUES, RESET_SPEED_FRACTION); motionProxy.Move(0, 0, 0); } } motionProxy.Disconnect(); }