コード例 #1
0
        public SpeedControl(double start, double end, double acceleration = 150, double topSpeed = 150)
        {
            //set fields
            _start        = start;
            _end          = end;
            _acceleration = acceleration;
            _topSpeed     = topSpeed;

            //set acceleration and dwell time
            _accelerationTime = Kinamatic.GetTimeUsingVelocity(0, _topSpeed, _acceleration);
            var accDist  = Kinamatic.GetDistanceUsingAcceleration(0, _acceleration, _accelerationTime);
            var halfDist = (double)Math.Abs(_end - _start) / 2d;

            if (accDist <= halfDist)
            {
                var dwellDist = (halfDist * 2) - (accDist * 2);
                _dwellTime    = Kinamatic.GetTimeUsingDistance(_topSpeed, 0, dwellDist);
                _peakVelocity = _topSpeed;
            }
            else
            {
                _accelerationTime = Kinamatic.GetTimeUsingDistance(0, _acceleration, halfDist);
                _dwellTime        = 0;
                _peakVelocity     = Kinamatic.GetVelocityUsingDistance(0, _acceleration, halfDist);
            }
            _totalTime = (_accelerationTime * 2) + _dwellTime;
        }
コード例 #2
0
        public double GetPosition(double time)
        {
            //validate
            if (time <= 0)
            {
                return(_start);
            }
            if (time >= _totalTime)
            {
                return(_end);
            }

            //acceleration
            var duration = Math.Min(time, _accelerationTime);
            var position = Kinamatic.GetDistanceUsingAcceleration(0, _acceleration, duration);

            //dwell
            if (time > _accelerationTime)
            {
                duration  = Math.Min(time - _accelerationTime, _dwellTime);
                position += Kinamatic.GetDistanceUsingAcceleration(_peakVelocity, 0, duration);
            }

            //deceleration
            if (time > _accelerationTime + _dwellTime)
            {
                duration  = Math.Min(time - (_accelerationTime + _dwellTime), _accelerationTime);
                position += Kinamatic.GetDistanceUsingAcceleration(_peakVelocity, -_acceleration, duration);
            }

            if (_start < _end)
            {
                position = _start + position;
            }
            else
            {
                position = _start - position;
            }

            return(position);
        }