예제 #1
0
        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);
        }
예제 #2
0
        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;
            }
        }
예제 #3
0
        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;
            }
        }