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();
}
