public override void Loop()
{
var img = Eye.GetLatestData();
var faces = Brain.DetectFaces(img);
if (faces.Length > 0)
{
var firstFace = faces.First();
// map the location
var x = firstFace.FaceRectangle.Left + firstFace.FaceRectangle.Width / 2;
var y = firstFace.FaceRectangle.Top + firstFace.FaceRectangle.Height / 2;
var coordinateX = 80;
var coordinateZ = 60 + 120 * (1 - (y * 1.0) / img.Height);
var pose = Arm.ConvertToPose(new Tuple <double, double, double>(coordinateX, 0, coordinateZ));
var rotate = 45 + 90 * (1 - x * 1.0 / img.Width);
var rotateStep = Arm.AngleToMM(rotate);
pose.MotorThreeSteps = rotateStep;
// Command ARM
Arm.MoveTo(pose);
}
else
{
Console.WriteLine("Face is not detected!");
}
//Thread.Sleep(10);
}
public void ExecuteNextLeftArmCommand()
{
lock (SyncRoot)
{
LeftArmCurrentCommand = LeftArmCommandList.Dequeue();
LeftArmCurrentStatus = Status.Executing;
}
switch (LeftArmCurrentCommand.Type)
{
case CommandType.Pose:
var command = LeftArmCurrentCommand as PoseCommand;
command.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
LeftArm.MoveTo(new PosePosition()
{
MotorOneSteps = command.NextPosePosition.X, MotorTwoSteps = command.NextPosePosition.Y, MotorThreeSteps = command.NextPosePosition.Z
});
break;
case CommandType.GCode:
var gcommand = LeftArmCurrentCommand as GCommand;
gcommand.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
var newPose = new PosePosition()
{
MotorOneSteps = LeftArm.CurrentPose.X + gcommand.XDelta,
MotorTwoSteps = LeftArm.CurrentPose.Y + gcommand.YDelta,
MotorThreeSteps = LeftArm.CurrentPose.Z + gcommand.ZDelta,
};
LeftArm.MoveTo(newPose);
break;
default:
lock (SyncRoot)
{
LeftArmCurrentCommand = null;
LeftArmCurrentStatus = Status.Idle;
}
break;
}
}
public void ExecuteNextCommand()
{
lock (SyncRoot)
{
CurrentCommand = CommandList.Dequeue();
CurrentStatus = Status.Executing;
}
switch (CurrentCommand.Type)
{
case CommandType.Pose:
var command = CurrentCommand as PoseCommand;
command.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
Arm.MoveTo(new PosePosition()
{
MotorOneSteps = command.NextPosePosition.X, MotorTwoSteps = command.NextPosePosition.Y, MotorThreeSteps = command.NextPosePosition.Z
});
break;
case CommandType.GCode:
var gcommand = CurrentCommand as GCommand;
gcommand.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
var newPose = new PosePosition()
{
MotorOneSteps = Arm.CurrentPose.X + gcommand.XDelta,
MotorTwoSteps = Arm.CurrentPose.Y + gcommand.YDelta,
MotorThreeSteps = Arm.CurrentPose.Z + gcommand.ZDelta,
};
Arm.MoveTo(newPose);
break;
case CommandType.Vision:
var vcommand = CurrentCommand as VisionCommand;
HandleVisionTask(vcommand);
break;
case CommandType.WaitingForVisionAnalyze:
// TODO: Check Server Status;
break;
case CommandType.WaitingProb:
lock (SyncRoot)
{
CurrentStatus = Status.WaitingProb;
}
break;
case CommandType.WaitingForTouch:
lock (SyncRoot)
{
CurrentStatus = Status.WaitingTouch;
}
break;
case CommandType.Pause:
lock (SyncRoot)
{
CurrentStatus = Status.Pause;
}
break;
case CommandType.Done:
lock (SyncRoot)
{
CurrentStatus = Status.TaskDone;
}
break;
default:
lock (SyncRoot)
{
CurrentCommand = null;
CurrentStatus = Status.Idle;
}
break;
}
}
