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;
}
private void _initVerticalStepPhase(WipFoot foot)
{
_initPhaseTime = _currentTime;
_initPhaseFootPosition = foot.Y; // 1000f;
_initPhaseFootVelocity = foot.dv;
}
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;
}