private void _changeGaitState()
{
if (_rightFoot.state == FootStates.ON_THE_FLOOR &&
_leftFoot.state == FootStates.ON_THE_FLOOR)
{
_gait = GaitState.STATIONARY;
_resetSteps();
_virtualVelocity = 0.0f;
_leftFoot.supportedBetweenSteps = false;
_rightFoot.supportedBetweenSteps = false;
printDebug("PAROU");
_virtualVelocityList = new List <float>();
}
else if ((_rightFoot.state == FootStates.ON_THE_FLOOR ||
_leftFoot.state == FootStates.ON_THE_FLOOR) &&
_gait == GaitState.STATIONARY)
{
_gait = GaitState.MOVING;
}
}
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;
}
