private void Update()
{
switch (_machineState)
{
case MachineState.Idle:
if (_instructionStack.Count > 0)
{
_lastTargetMachineState = _currentInstruction.TargetMachineState;
_currentInstruction = _instructionStack.Pop();
_animationTime = 0f;
_machineState = MachineState.DoingControlledMoves;
}
break;
case MachineState.DoingControlledMoves:
_animationTime += Time.deltaTime;
var normalizedAnimationValue = _animationTime / _currentInstruction.MoveTime;
var animatedMachineState = _lastTargetMachineState.LerpTowards(
_currentInstruction.TargetMachineState,
(1 + Mathf.Cos((1 - normalizedAnimationValue) * Mathf.PI)) / 2f);
AddToOriginState(animatedMachineState);
if (_animationTime > _currentInstruction.MoveTime)
{
_machineState = MachineState.Idle;
}
break;
}
}
public void AddToOriginState(LLMachineState diffState)
{
// NOTE: We are using setting the state via diffState because the real machine will only
// work with diffStates and the ModelMachine has to behave exactly the same way as the real machine.
var state = Constants.OriginMachineState + diffState;
_motorizedArm1.UpdateState(state.Motor1Rotation, InverseKinematics.CalculateJoint2RotationFromJoint1Rotation(state.Motor1Rotation, 0f));
_motorizedArm2.UpdateState(state.Motor2Rotation, InverseKinematics.CalculateJoint2RotationFromJoint1Rotation(state.Motor2Rotation, 0f));
_motorizedArm3.UpdateState(state.Motor3Rotation, InverseKinematics.CalculateJoint2RotationFromJoint1Rotation(state.Motor3Rotation, 0f));
_motorizedArm4.UpdateState(state.Motor4Rotation, InverseKinematics.CalculateJoint2RotationFromJoint1Rotation(state.Motor4Rotation, 0f));
}
/// <summary>
/// Lerps LLMachineState a towards b by value.
/// </summary>
public static LLMachineState LerpTowards(this LLMachineState a, LLMachineState b, float value)
{
var m1Rot = Mathf.Lerp(a.Motor1Rotation, b.Motor1Rotation, value);
var m2Rot = Mathf.Lerp(a.Motor2Rotation, b.Motor2Rotation, value);
var m3Rot = Mathf.Lerp(a.Motor3Rotation, b.Motor3Rotation, value);
var m4Rot = Mathf.Lerp(a.Motor4Rotation, b.Motor4Rotation, value);
var m1J2Rot = Mathf.Lerp(a.Arm1Joint2Rotation, b.Arm1Joint2Rotation, value);
var m2J2Rot = Mathf.Lerp(a.Arm2Joint2Rotation, b.Arm2Joint2Rotation, value);
var m3J2Rot = Mathf.Lerp(a.Arm3Joint2Rotation, b.Arm3Joint2Rotation, value);
var m4J2Rot = Mathf.Lerp(a.Arm4Joint2Rotation, b.Arm4Joint2Rotation, value);
return(new LLMachineState(m1Rot, m2Rot, m3Rot, m4Rot, m1J2Rot, m2J2Rot, m3J2Rot, m4J2Rot));
}
public void Instruct(List <LLInstruction> instructions)
{
var levelingInstructions = instructions.Where(instruction => instruction.IsLevelingInstruction);
// NOTE: The current Max amount of instructions which can be sent in one go is 100.
var diffInstructionList = instructions
.Take(100)
.Select(instruction =>
new LLInstruction(
instruction.TargetMachineState - Constants.OriginMachineState + _levelingOffset,
instruction.MoveTime))
.ToList();
foreach (var instruction in levelingInstructions)
{
var levelingOnlyState = instruction.TargetMachineState -
new HLInstruction(0.01f, 0f, 0f, 0.2f).Translate().TargetMachineState;
_levelingOffset += levelingOnlyState;
}
SendInstructions(diffInstructionList);
}
