public override void Run(CancellationToken cancellationToken)
        {
            ThrowExceptionIfLidarCollectorIsStoppedOrSensorError();

            float angleToLargestDistance = _lidar.LargestDistanceInRange(260, 100).Angle;

            if (float.IsNaN(angleToLargestDistance))
            {
                _wheels.Stop();
                Debug.WriteLine("STOPPED due to no LIDAR distance found in range!", "ControlLogic");
                Thread.Sleep(200);
            }
            else
            {
                _speedLeft  = BaseSpeed;
                _speedRight = BaseSpeed;

                CompensateSpeedTowardsAngle(angleToLargestDistance);

                CompensateSpeedFromSideClearance();

                if (_ultrasonic.Fwd < clearanceLimitFwd)
                {
                    Debug.WriteLine("Emergency steer Fwd started.", "ControlLogic");
                    EmergencySteerFromObstacleInFront(1.3f, 50, cancellationToken);
                    Debug.WriteLine("Emergency steer Fwd completed.", "ControlLogic");
                }
                else
                {
                    _wheels.SetSpeed(_speedLeft, _speedRight);
                }
            }

            Thread.Sleep(50);
        }
Пример #2
0
        public override void Run(CancellationToken cancellationToken)
        {
            ThrowExceptionOnSensorError();

            if (_ultrasonic.Fwd < 0.5)
            {
                _wheels.Stop();
                Thread.Sleep(500);
                RotateAwayFromObstruction(cancellationToken, desiredFrontalClearance: 1.0f, rotationSpeedPercentage: 50);
            }
            else
            {
                float reductionRate = 1.0f;
                KeepCenterCorridor(reductionRate);
            }
        }
Пример #3
0
        public override void Run(CancellationToken cancellationToken)
        {
            ThrowExceptionIfCollectorIsStopped();

            const int rotateLimit = 60;

            float angleToLargestDistance = _lidar.LargestDistance.Angle;

            if (float.IsNaN(angleToLargestDistance))
            {
                _wheels.Stop();
                Debug.WriteLine("STOPPED WHEELS due to no LIDAR distance found!", "ControlLogic");
                Thread.Sleep(200);
            }
            else if (angleToLargestDistance > rotateLimit && angleToLargestDistance < 360 - rotateLimit)
            {
                TurnTowardsLargestDistance(cancellationToken);
            }

            Thread.Sleep(50);
        }
Пример #4
0
        public override void Run(CancellationToken cancellationToken)
        {
            ThrowExceptionIfLidarCollectorIsStoppedOrSensorError();

            float angleToLargestDistance = _lidar.LargestDistanceInRange(260, 100).Angle;

            if (float.IsNaN(angleToLargestDistance))
            {
                _wheels.Stop();
                Debug.WriteLine("STOPPED WHEELS due to no LIDAR distance found in range!", "ControlLogic");
                Thread.Sleep(200);
            }
            else
            {
                SteerTowardsAngle(angleToLargestDistance, 100);
            }

            Thread.Sleep(50);
        }