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));
        }
Ejemplo n.º 3
0
        /// <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));
        }
Ejemplo n.º 4
0
        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);
        }