Пример #1
0
        private void _endVerticalStepPhase(WipFoot foot)
        {
            _endPhaseTime         = _currentTime;
            _endPhaseFootPosition = foot.Y; // 1000f;

            _totalStepPhaseTime = ((float)_endPhaseTime - (float)_initPhaseTime) / 1000.0f;

            if (_maxHeightPhasePos < _endPhaseFootPosition)
            {
                _maxHeightPhasePos = _endPhaseFootPosition;
            }

            _endPhaseFootVelocity = (_endPhaseFootPosition - _initPhaseFootPosition) / _totalStepPhaseTime;
        }
Пример #2
0
 private void _initVerticalStepPhase(WipFoot foot)
 {
     _initPhaseTime         = _currentTime;
     _initPhaseFootPosition = foot.Y; // 1000f;
     _initPhaseFootVelocity = foot.dv;
 }
Пример #3
0
        public WalkingInPlace(string logfilename)
        {
            velFileWriter = new FileWriter("out.csv");
            velFileWriter.write("user height $ distance $ foot vel $ speed");

            _rotationTime = new Stopwatch();
            _rotationTime.Start();


            _m = 2.83f;
            _b = 10f;


            _veFrameRate   = 60;
            _angularOffset = 0f;

            _minRotationThreshold = 0.085f;
            _maxRotationThreshold = 0.90f;

            _userHeight = 0f;

            _initialVelocity = 0.24f;
            _maxVelocity     = 3.0f;

            _gait   = GaitState.STATIONARY;
            wipMode = WiPMode.NORMAL;

            computeAngularVelocityMode = ComputeAngularVelocityMode.LINEAR;
            //wipMode = WiPMode.SWING_UP_VELOCITY_ONLY;

            angularVelocityMode = AngularVelocityMode.LINEAR_VARIABLE;

            _positionThreshold = 0.035f;
            _rightFoot         = new WipFoot(Type.RIGHT);
            _leftFoot          = new WipFoot(Type.LEFT);

            _virtualVelocity     = 0.0f;
            _virtualVelocityList = new List <float>();
            _dispatchVelocity    = 0f;

            _angularPosition = 0f;

            _gaitBackwards = false;

            _resetSteps();

            _timeout = new Timeout(1000);

            _distance = 0.0f;
            _footVel  = 0.0f;

            _positionThresholdAdd = 90f;

            _maxHeightSpeedThreshold = 0.10f; //TODO
            _stepInitSpeedThreshold  = 0.0f;
            _stepMaxHeightPos        = 0.0f;
            _maxHeightPhasePos       = 0.0f;
            _lastPhasePos            = 0.0f;
            _firstPhasePos           = 0.0f;

            _initPhaseTime         = 0;;
            _initPhaseFootPosition = 0.0f;
            _initPhaseFootVelocity = 0.0f;

            _endPhaseTime         = 0;
            _endPhaseFootPosition = 0.0f;
            _endPhaseFootVelocity = 0.0f;

            _totalStepPhaseTime    = 0.0f;
            _lastStepPhaseTime     = 0.0f;
            _lastStepPhaseVelocity = 0.0f;

            _floorTimeInit = 0;
            _floorTimeEnd  = 0;

            ConstantAngularVelocityValue = 30.0f;

            _resetSteps();

            OutputEnabled = false;
        }