Пример #1
0
 public void MoveLegs(LegPositions position)
 {
     MoveLeftFrontLeg(position.LeftFront);
     MoveRightFrontLeg(position.RightFront);
     MoveLeftRearLeg(position.LeftRear);
     MoveRightRearLeg(position.RightRear);
 }
Пример #2
0
 public SmartQuadrupedController(QuadrupedIkDriver driver)
 {
     _driver = driver ?? throw new ArgumentNullException(nameof(driver));
     _driver.Setup();
     _lastWrittenPosition = _driver.ReadCurrentLegPositions();
     Task.Run(MainControllerLoop).ConfigureAwait(false);
 }
Пример #3
0
 public bool MoveFinished(LegPositions other)
 {
     return(LeftFront == other.LeftFront &&
            RightFront == other.RightFront &&
            LeftRear == other.LeftRear &&
            RightRear == other.RightRear);
 }
Пример #4
0
 private async Task ExecuteMove(LegPositions targetPosition)
 {
     while (!_lastWrittenPosition.MoveFinished(targetPosition))
     {
         _lastWrittenPosition = _lastWrittenPosition.MoveTowards(targetPosition, Speed * 0.001f * _updateDelay);
         _driver.MoveLegsSynced(_lastWrittenPosition);
         await Task.Delay(_updateDelay);
     }
 }
Пример #5
0
        public LegPositions MoveTowards(LegPositions target, float distance)
        {
            var newLeftFront  = LeftFront.MoveTowards(target.LeftFront, distance);
            var newRightFront = RightFront.MoveTowards(target.RightFront, distance);
            var newLeftRear   = LeftRear.MoveTowards(target.LeftRear, distance);
            var newRightRear  = RightRear.MoveTowards(target.RightRear, distance);

            return(new LegPositions(newLeftFront, newRightFront, newLeftRear, newRightRear));
        }
Пример #6
0
 public InterpolationGaitEngine(QuadrupedIkDriver driver) : base(driver)
 {
     Driver.Setup();
     _lastWrittenPosition = Driver.ReadCurrentLegPositions();
     if (_moves.TryDequeue(out var deqeueuedLegPosition))
     {
         _nextMove = deqeueuedLegPosition;
     }
     StartEngine();
 }
Пример #7
0
        public void MoveLegsSynced(LegPositions position)
        {
            if (position == null)
            {
                throw new ArgumentNullException(nameof(position));
            }
            var rightFrontLegGoalPositions = CalculateIkForLeg(position.RightFront, RightFront);
            var rightRearLegGoalPositions  = CalculateIkForLeg(position.RightRear, RightRear);
            var leftFrontLegGoalPositions  = CalculateIkForLeg(position.LeftFront, LeftFront);
            var leftRearLegGoalPositions   = CalculateIkForLeg(position.LeftRear, LeftRear);

            byte[]  servoIds   = new byte[12];
            float[] servoGoals = new float[12];

            // Pair motor ids with their respective goal positions
            servoIds[0]   = RightFront.CoxaId;
            servoGoals[0] = rightFrontLegGoalPositions.Coxa;
            servoIds[1]   = RightFront.FemurId;
            servoGoals[1] = rightFrontLegGoalPositions.Femur;
            servoIds[2]   = RightFront.TibiaId;
            servoGoals[2] = rightFrontLegGoalPositions.Tibia;

            servoIds[3]   = RightRear.CoxaId;
            servoGoals[3] = rightRearLegGoalPositions.Coxa;
            servoIds[4]   = RightRear.FemurId;
            servoGoals[4] = rightRearLegGoalPositions.Femur;
            servoIds[5]   = RightRear.TibiaId;
            servoGoals[5] = rightRearLegGoalPositions.Tibia;

            servoIds[6]   = LeftFront.CoxaId;
            servoGoals[6] = leftFrontLegGoalPositions.Coxa;
            servoIds[7]   = LeftFront.FemurId;
            servoGoals[7] = leftFrontLegGoalPositions.Femur;
            servoIds[8]   = LeftFront.TibiaId;
            servoGoals[8] = leftFrontLegGoalPositions.Tibia;

            servoIds[9]    = LeftRear.CoxaId;
            servoGoals[9]  = leftRearLegGoalPositions.Coxa;
            servoIds[10]   = LeftRear.FemurId;
            servoGoals[10] = leftRearLegGoalPositions.Femur;
            servoIds[11]   = LeftRear.TibiaId;
            servoGoals[11] = leftRearLegGoalPositions.Tibia;

            lock (_driver.SyncLock)
            {
                _driver.GroupSyncSetGoalPositionInDegrees(servoIds, servoGoals);
            }
        }
Пример #8
0
        public FunctionalGaitEngine(QuadrupedIkDriver driver) : base(driver)
        {
            Driver.Setup();
            var   currentPosition = driver.ReadCurrentLegPositions();
            float average         = (currentPosition.LeftFront.Z +
                                     currentPosition.RightFront.Z +
                                     currentPosition.LeftRear.Z +
                                     currentPosition.RightRear.Z) / 4;

            if (average > -9)
            {
                Driver.MoveLegsSynced(RelaxedStance.Transform(new Vector3(0, 0, -LegHeight)));
                Thread.Sleep(1000);
            }
            _lastWrittenPosition = RelaxedStance;
            Driver.MoveLegsSynced(_lastWrittenPosition);
            StartEngine();
        }
Пример #9
0
        protected override void EngineSpin()
        {
            // if last telemetrics was too long time ago fire
            var currentTickCount = Environment.TickCount;

            if (currentTickCount - _lastTelemetricsUpdate > TelemetricsUpdateInterval)
            {
                _lastTelemetricsUpdate = currentTickCount;
                NewTelemetricsUpdate?.Invoke(this, Driver.ReadTelemetrics());
            }
            if (_lastWrittenPosition.MoveFinished(_nextMove ?? _lastWrittenPosition))
            {
                if (_moves.TryDequeue(out var deqeueuedLegPosition))
                {
                    _moveQueueSingal.Reset();
                    _nextMove = deqeueuedLegPosition;
                }
                else
                {
                    if (_moves.IsEmpty)
                    {
                        _moveQueueSingal.Set();
                    }
                    return;
                }
            }
            try
            {
                _lastWrittenPosition = _lastWrittenPosition.MoveTowards(_nextMove, NextStepLength);
                Driver.MoveLegsSynced(_lastWrittenPosition);
            }
            catch (IOException e)
            {
                GaitEngineError?.Invoke(this, e);
            }
        }
Пример #10
0
        private LegFlags DetermineBestLeg(LegPositions currentPositions)
        {
            Vector2 rightFrontProfile = currentPositions.RightFront.ToDirectionVector2().Normal();
            Vector2 rightRearProfile  = currentPositions.RightRear.ToDirectionVector2().Normal();
            Vector2 leftFrontProfile  = currentPositions.LeftFront.ToDirectionVector2().Normal();
            Vector2 leftRearProfile   = currentPositions.LeftRear.ToDirectionVector2().Normal();

            var relaxed = RelaxedStance;

            double rightFrontValue = AreaOfTriangle(rightRearProfile, leftFrontProfile, relaxed.LeftRear.ToDirectionVector2().Normal());
            double rightRearValue = AreaOfTriangle(rightFrontProfile, leftRearProfile, relaxed.LeftFront.ToDirectionVector2().Normal());
            double leftFrontValue = AreaOfTriangle(rightFrontProfile, leftRearProfile, relaxed.RightRear.ToDirectionVector2().Normal());
            double leftRearValue = AreaOfTriangle(rightRearProfile, leftFrontProfile, relaxed.RightFront.ToDirectionVector2().Normal());
            double highest = new double[] { rightFrontValue, rightRearValue, leftFrontValue, leftRearValue }.Max();

            if (highest == rightFrontValue)
            {
                return(LegFlags.RightFront);
            }
            else if (highest == rightRearValue)
            {
                return(LegFlags.RightRear);
            }
            else if (highest == leftFrontValue)
            {
                return(LegFlags.LeftFront);
            }
            else if (highest == leftRearValue)
            {
                return(LegFlags.LeftRear);
            }
            else
            {
                throw new InvalidOperationException();
            }
        }
Пример #11
0
 public void AddStep(LegPositions nextStep)
 {
     _moves.Enqueue(nextStep);
     _moveQueueSingal.Reset();
 }
Пример #12
0
        protected override void EngineSpin()
        {
            double angle      = TimeSinceStart / 1000.0 * 280.0; // this determins the speed of switching legs
            double legLiftSin = Math.Sin((angle * 2).DegreeToRad());
            double swaySin    = Math.Sin(angle.DegreeToRad());
            var    currentLeg = _legs[(int)(angle % 360 / 90)];

            // shift leg offsets
            // TODO refactor
            if (currentLeg == LegFlags.RightFront)
            {
                _rightFrontOffset += 3 * NextStepLength;
            }
            else
            {
                _rightFrontOffset -= NextStepLength;
            }
            if (currentLeg == LegFlags.RightRear)
            {
                _rightRearOffset += 3 * NextStepLength;
            }
            else
            {
                _rightRearOffset -= NextStepLength;
            }
            if (currentLeg == LegFlags.LeftFront)
            {
                _leftFrontOffset += 3 * NextStepLength;
            }
            else
            {
                _leftFrontOffset -= NextStepLength;
            }
            if (currentLeg == LegFlags.LeftRear)
            {
                _leftRearOffset += 3 * NextStepLength;
            }
            else
            {
                _leftRearOffset -= NextStepLength;
            }

            _lastWrittenPosition = RelaxedStance
                                   .Transform(new Vector3((float)(swaySin * 3) * _direction.Y, -(float)(swaySin * 3) * _direction.X, 0))
                                   .Transform(new Vector3(_rightFrontOffset * _direction.X, _rightFrontOffset * _direction.Y, 0), LegFlags.RightFront)
                                   .Transform(new Vector3(_rightRearOffset * _direction.X, _rightRearOffset * _direction.Y, 0), LegFlags.RightRear)
                                   .Transform(new Vector3(_leftFrontOffset * _direction.X, _leftFrontOffset * _direction.Y, 0), LegFlags.LeftFront)
                                   .Transform(new Vector3(_leftRearOffset * _direction.X, _leftRearOffset * _direction.Y, 0), LegFlags.LeftRear)
                                   .Transform(new Vector3(0, 0, (float)Math.Abs(legLiftSin) * 3), currentLeg);
            try
            {
                Driver.MoveLegsSynced(_lastWrittenPosition);
            }
            catch (IOException e)
            {
                Console.ForegroundColor = ConsoleColor.Red;
                Console.WriteLine(e);
                Console.ResetColor();
                throw;
            }
        }