예제 #1
0
        public async Task <bool> MoveToTarget(string ip, int port)
        {
            Stopwatch stopwatch = new Stopwatch();

            stopwatch.Start();
            var controlWordEP = new ControlWordController();
            var statusWordEP  = new StatusWordController();
            var motorEP       = new MotorController();

            uint acceleration   = Convert.ToUInt32(Acceleration * Precision);
            uint deceleration   = Convert.ToUInt32(Deceleration * Precision);
            uint velocity       = Convert.ToUInt32(TargetVelocity * Precision);
            int  targetPosition = Convert.ToInt32(TargetPosition * Precision);

            // Set operation mode
            motorEP.SetOperationMode(ip, port, 1);
            Stopwatch operationMode = new Stopwatch();

            operationMode.Start();
            while (motorEP.GetOperationModeDisplay(ip, port) != 1)
            {
                Debug.WriteLine("OPM wrong");
                await Delay(10);
            }
            operationMode.Stop();
            Debug.WriteLine($"OperationMode Set: {operationMode.ElapsedMilliseconds}");

            // Check if start is reseted
            ControlWord controlWord = new ControlWord();

            controlWord = controlWordEP.GetControlWord(ip, port);
            if (controlWord.Bit04)
            {
                Debug.WriteLine("Reset bit 4, Z190");
                controlWord.Bit04 = false;
                controlWordEP.SetControlWord(ip, port, controlWord);
            }

            // Set relative positioning
            if (RelativePosition)
            {
                if (!controlWord.Bit06)
                {
                    controlWord.Bit06 = true;
                    controlWordEP.SetControlWord(ip, port, controlWord);
                    // Check relative positioning is set (Bit 6)
                    controlWord = controlWordEP.GetControlWord(ip, port);
                    if (controlWord.Bit06 != RelativePosition)
                    {
                        return(false);
                    }
                }
            }
            else
            {
                if (controlWord.Bit06)
                {
                    controlWord.Bit06 = false;
                    controlWordEP.SetControlWord(ip, port, controlWord);
                    // Check relative positioning is set (Bit 6)
                    controlWord = controlWordEP.GetControlWord(ip, port);
                    if (controlWord.Bit06 != RelativePosition)
                    {
                        return(false);
                    }
                }
            }


            int positionWindow = motorEP.GetPositionWindow(ip, port);

            // Set acceleration
            motorEP.SetProfileAcceleration(ip, port, acceleration);
            if (acceleration != motorEP.GetProfileAcceleration(ip, port))
            {
                return(false);
            }

            // Set deceleration
            motorEP.SetProfileDeceleration(ip, port, deceleration);
            if (deceleration != motorEP.GetProfileDeceleration(ip, port))
            {
                return(false);
            }

            // Set velocity
            motorEP.SetProfileVelocity(ip, port, velocity);
            if (velocity != motorEP.GetProfileVelocity(ip, port))
            {
                return(false);
            }

            // Set target position
            motorEP.SetTargetPosition(ip, port, targetPosition);
            if (targetPosition != motorEP.GetTargetPosition(ip, port))
            {
                return(false);
            }

            // Check controlWord bit 4 (Start)
            controlWord = controlWordEP.GetControlWord(ip, port);
            if (controlWord.Bit04)
            {
                Debug.WriteLine("Reset bit 4 Z243");
                controlWord.Bit04 = false;
                controlWordEP.SetControlWord(ip, port, controlWord);
                await Delay(50);
            }
            ////////////////// Debug
            stopwatch.Stop();
            Debug.WriteLine($"Instruction Time: {stopwatch.ElapsedMilliseconds}");
            stopwatch = new Stopwatch();
            stopwatch.Start();
            //////////////////

            // Start
            controlWord.Bit04 = true;
            controlWordEP.SetControlWord(ip, port, controlWord);
            await Delay(50);

            // Check statusWord bit 12
            while (!NewTargetSet(statusWordEP.GetStatusWord(ip, port)))
            {
                Debug.WriteLine("Check bit 12");
                await Delay(50);
            }

            // Reset start
            controlWord.Bit04 = false;
            controlWordEP.SetControlWord(ip, port, controlWord);

            while (!TargetReached(statusWordEP.GetStatusWord(ip, port)) && !InPositionWindow(targetPosition, motorEP.GetActualPosition(ip, port), positionWindow))
            {
                Stopwatch targetReached = new Stopwatch();
                targetReached.Start();
                if (TargetReached(statusWordEP.GetStatusWord(ip, port)) && !InPositionWindow(targetPosition, motorEP.GetActualPosition(ip, port), positionWindow))
                {
                    Debug.WriteLine("--------------------------");
                    Debug.WriteLine("-----------Error----------");
                    Debug.WriteLine("--------------------------");
                }
                targetReached.Stop();
                Debug.WriteLine($"Target check time: {targetReached.ElapsedMilliseconds}");
                await Delay(50);
            }

            stopwatch.Stop();
            Debug.WriteLine($"Movement Time: {stopwatch.ElapsedMilliseconds}");
            return(true);
        }