﻿using System;
using System.Numerics;

namespace DynamixelServo.Quadruped
{
    public class BasicQuadrupedGaitEngine : QuadrupedGaitEngine
    {
        private const int Speed = 10;

        private const int MaxForward = 5;
        private const int MinForward = -5;

        private const int LegDistance = 15;
        private const int LegHeight = -13;

        private bool _movingForward;
        private Vector3 _lastWrittenPosition = Vector3.Zero;

        public BasicQuadrupedGaitEngine(QuadrupedIkDriver driver) : base(driver)
        {
            Driver.Setup();
            Driver.StandUpfromGround();
            StartEngine();
        }

        protected override void EngineSpin()
        {
            var translate = Vector3.Zero;
            if (_movingForward)
            {
                translate.Y += Speed * 0.001f * TimeSincelastTick;
            }
            else
            {
                translate.Y -= Speed * 0.001f * TimeSincelastTick;
            }
            _lastWrittenPosition += translate;
            if (_movingForward && _lastWrittenPosition.Y > MaxForward)
            {
                _movingForward = false;
            }
            else if (!_movingForward && _lastWrittenPosition.Y < MinForward)
            {
                _movingForward = true;
            }
            Driver.MoveAbsoluteCenterMass(_lastWrittenPosition, LegDistance, LegHeight);
        }
    }
}

﻿using System;
using System.Numerics;

namespace DynamixelServo.Quadruped
{
    public class BasicQuadrupedGaitEngine : QuadrupedGaitEngine
    {
        private const int Speed = 12;

        private int _currentIndex;

        private readonly Vector3[] _positions =
        {
            new Vector3(-5, 5, 3),
            new Vector3(5, 5, -3),
            new Vector3(5, -5, 3),
            new Vector3(-5, -5, -3)
        };

        private const float LegHeight = -11f;
        private const int LegDistance = 15;

        private Vector3 _lastWrittenPosition = Vector3.Zero;

        public BasicQuadrupedGaitEngine(QuadrupedIkDriver driver) : base(driver)
        {
            Driver.Setup();
            Driver.StandUpfromGround();
            StartEngine();
        }

        protected override void EngineSpin()
        {
            if (_lastWrittenPosition.Similar(_positions[_currentIndex], 0.25f))
            {
                _currentIndex++;
                if (_currentIndex >= _positions.Length)
                {
                    _currentIndex = 0;
                }
            }
            _lastWrittenPosition =
                _lastWrittenPosition.MoveTowards(_positions[_currentIndex], Speed * 0.001f * TimeSincelastTick);
            Driver.MoveAbsoluteCenterMass(_lastWrittenPosition, LegDistance, LegHeight);
        }
    }
}

Add simple interpolated motion driver
Add simple interpolated motion driver
