예제 #1
0
        private DriveCommand DriveNoBallDetected(TennisBall ball)
        {
            // Sanity check
            if (ball != null)
            {
                return(null);
            }


            Console.Write("Ball not detected | ");

            // Clear verification queue if it has values
            this.verificationQueue.Clear();

            GPS    ascent         = AscentPacketHandler.GPSData;
            double distanceToGate = ascent.GetDistanceTo(this.gate);

            // Kick back to GPS
            if (distanceToGate > DISTANCE_SWITCH_TO_GPS)    // 6 meters
            {
                Console.WriteLine("Distance: " + distanceToGate + ". Switch to GPS");

                switchToGPS = true;
                return(DriveCommand.Straight(Speed.HALT));
            }

            // Turn to face heading, drive toward it
            if (distanceToGate > DISTANCE_USE_HEADING)      // 3 meters
            {
                Console.WriteLine("Distance: " + distanceToGate + ". Turning toward heading to drive towrad it");

                short  ascentHeading = AscentPacketHandler.Compass;
                double headingToGate = ascent.GetHeadingTo(this.gate);

                Console.Write("currCompass: "******" | headingToGoal: " + headingToGate + " | distance: " + distanceToGate + " | ");

                // Aligned with heading. Start going straight
                if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, Scan.HEADING_THRESHOLD))
                {
                    return(DriveCommand.Straight(Speed.VISION));
                }

                // Turn toward gate heading angle
                if (IMU.IsHeadingWithinThreshold(ascentHeading, (headingToGate + 90) % 360, 90))
                {
                    return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
                }
                else
                {
                    return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                }

                // Probably would work, kept as reference

                /*
                 * double lowBound = headingToGate;
                 * double highBound = (headingToGate + 180) % 360;
                 *
                 * if (lowBound < highBound)
                 * {
                 *  if (lowBound < ascentHeading && ascentHeading < highBound)
                 *  {
                 *      return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 *  else
                 *  {
                 *      return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 * }
                 * else
                 * {
                 *  if (!(highBound < ascentHeading && ascentHeading < lowBound))
                 *  {
                 *      return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 *  else
                 *  {
                 *      return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 * }
                 */
            }

            // If scanning, complete scan
            if (this.scan != null)
            {
                Console.WriteLine("Scanning... Distance: " + distanceToGate);

                if (!this.scan.IsComplete())
                {
                    return(scan.FindNextDriveCommand());
                }
                else
                {
                    this.completedScans++;

                    // Clear scan, will rescan below
                    this.scan = null;
                }
            }

            switch (this.completedScans)
            {
            case 0:
            {
                // Initialize the first scan
                if (distanceToGate > DISTANCE_CLOSE_RANGE)          // 2 meters
                {
                    // Turn toward heading. Scan, use heading as reference
                    StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
                    Console.WriteLine("Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));

                    this.scan = new Scan(this.gate, true);
                }
                else
                {
                    // Scan
                    StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
                    Console.WriteLine("Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));

                    this.scan = new Scan(this.gate, false);
                }

                break;
            }

            case 1:
            {
                // Align toward heading, drive for 10ish seconds,
                StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 1st 10s scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
                Console.WriteLine("Scan: Distance: " + Math.Round(distanceToGate, 2) + ". Scanning (using heading). Driving 5m away...");

                this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);

                break;
            }

            /*
             * case 2:
             * {
             *  // Broaden HSV values, scan
             *  StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
             *  Console.WriteLine("Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
             *
             *  this.camera.BroadenHsvValues();
             *
             *  this.scan = new Scan(this.gate, false);
             *
             *  break;
             * }
             */
            case 2:
            {
                // Align toward heading (again), drive for 5ish seconds,
                StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
                Console.WriteLine("Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));

                this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);

                break;
            }

            case 3:
            {
                // We've already run 1 scan, 1 5m scan, 1 broaden HSV, 1 more 5m scan, so drive straight to kick back to GPS and do it over again
                return(DriveCommand.Straight(Speed.VISION));
            }

            default:
            {
                break;
            }
            }

            return(scan.FindNextDriveCommand());
        }
예제 #2
0
        /// <summary>
        /// Given a Plot representing the obstacles, find the Line representing the best gap.
        /// Do GPS driving according to our rendition of the "Follow the Gap" algorithm:
        /// Reference here: https://pdfs.semanticscholar.org/c432/3017af7bce46fc7574ada008b8af1011e614.pdf
        /// This algorithm avoids obstacles by finding the gap between them. It has a threshold gap (i.e. robot width),
        /// and if the measured gap is greater than the threshold gap, the robot follows the calculated gap angle.
        /// In our case, the best gap will also be the one with the smallest displacement from the goal (the gate).
        /// </summary>
        public Line FindBestGap(Plot obstacles)
        {
            List <Region> regions   = obstacles.Regions;
            Line          bestGap   = null;
            double        bestAngle = Double.MaxValue;

            // Get GPS, heading data
            GPS   currGPS     = AscentPacketHandler.GPSData;
            short currCompass = AscentPacketHandler.Compass;

            // Add distance to averaging queue
            double distance = currGPS.GetDistanceTo(this.gate);

            while (this.averagingQueue.Count >= AVERAGING_QUEUE_CAPACITY)
            {
                double value;
                this.averagingQueue.TryDequeue(out value);
            }
            this.averagingQueue.Enqueue(distance);

            // Heading from current GPS to gate GPS
            double idealDirection = currGPS.GetHeadingTo(gate);

            // Check first and last Region gaps (may be same Region if only one Region)
            Region firstRegion = regions.ElementAt(0);
            Region lastRegion  = regions.ElementAt(regions.Count - 1);

            // Left and right fields of view (FOV)
            Line lrfLeftFOV  = DriveContext.LRF_LEFT_FOV_EDGE;
            Line lrfRightFOV = DriveContext.LRF_RIGHT_FOV_EDGE;

            // Check if leftmost Coordinate in the leftmost Region is on the right half of the entire FOV. If it is, make leftEdgeCoordinate where the
            // max-acceptable-range meets the left FOV line, since the FindClosestPointOnLine function return will cause errors for 180 degree FOV.
            Coordinate leftEdgeCoordinate = Line.FindClosestPointOnLine(lrfLeftFOV, firstRegion.StartCoordinate);

            if (firstRegion.StartCoordinate.X > 0)
            {
                leftEdgeCoordinate = new Coordinate(-AutonomousService.OBSTACLE_DETECTION_DISTANCE, 0, CoordSystem.Cartesian);
            }

            // Checking the left edge gap
            Line leftEdgeGap = new Line(leftEdgeCoordinate, firstRegion.StartCoordinate);

            if (leftEdgeGap.Length >= DriveContext.ASCENT_WIDTH)
            {
                double     angle;
                Coordinate leftMidPoint = leftEdgeGap.FindMidpoint();

                if (leftMidPoint.X > 0)
                {
                    angle = currCompass + (90 - (Math.Atan2(leftMidPoint.Y, leftMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else if (leftMidPoint.X < 0)
                {
                    angle = currCompass - (90 - (Math.Atan2(-1 * leftMidPoint.Y, leftMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else
                {
                    // midpoint.X is 0
                    angle = currCompass;
                }

                if (Math.Abs(idealDirection - angle) < bestAngle)
                {
                    bestAngle = idealDirection - angle;
                    bestGap   = leftEdgeGap;
                }
            }

            // Check if rightmost Coordinate in the rightmost Region is on the left half of the entire FOV. If it is, make rightEdgeCoordinate where the
            // max-acceptable-range meets the right FOV line, since the FindClosestPointOnLine function return will cause errors for 180 degree FOV.
            Coordinate rightEdgeCoordinate = Line.FindClosestPointOnLine(lrfRightFOV, lastRegion.EndCoordinate);

            if (lastRegion.EndCoordinate.X < 0)
            {
                rightEdgeCoordinate = new Coordinate(AutonomousService.OBSTACLE_DETECTION_DISTANCE, 0, CoordSystem.Cartesian);
            }

            // Checking the right edge gap
            Line rightEdgeGap = new Line(rightEdgeCoordinate, lastRegion.EndCoordinate);

            if (rightEdgeGap.Length >= DriveContext.ASCENT_WIDTH)
            {
                double     angle;
                Coordinate rightMidPoint = rightEdgeGap.FindMidpoint();

                if (rightMidPoint.X > 0)
                {
                    angle = currCompass + (90 - (Math.Atan2(rightMidPoint.Y, rightMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else if (rightMidPoint.X < 0)
                {
                    angle = currCompass - (90 - (Math.Atan2(-1 * rightMidPoint.Y, rightMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else
                {
                    // midpoint.X is 0
                    angle = currCompass;
                }

                if (Math.Abs(idealDirection - angle) < bestAngle)
                {
                    bestAngle = angle;
                    bestGap   = rightEdgeGap;
                }
            }

            // Iterate through the rest of the gaps to find the bestGap by calculating valid (large enough) gaps as Line objects
            for (int i = 0; i < regions.Count - 2; i++) // don't iterate through entire list, will result in index out of bounds error on rightRegion assignment
            {
                Region leftRegion  = regions[i];
                Region rightRegion = regions[i + 1];

                // Gap is distance, just needs to be big enough (the width of the robot)
                double gap = Plot.GapDistanceBetweenRegions(leftRegion, rightRegion);
                if (gap >= DriveContext.ASCENT_WIDTH)
                {
                    Line gapLine = new Line(new Coordinate(leftRegion.EndCoordinate.X, leftRegion.EndCoordinate.Y, CoordSystem.Cartesian),
                                            new Coordinate(rightRegion.StartCoordinate.X, rightRegion.StartCoordinate.Y, CoordSystem.Cartesian));

                    Coordinate midpoint = gapLine.FindMidpoint();
                    if (bestGap == null)
                    {
                        bestGap = gapLine;
                    }
                    else
                    {
                        double angle;

                        if (midpoint.X > 0)
                        {
                            angle = currCompass + (90 - (Math.Atan2(midpoint.Y, midpoint.X) * (180 / Math.PI)));
                            angle = angle % 360;
                        }
                        else if (midpoint.X < 0)
                        {
                            angle = currCompass - (90 - (Math.Atan2(-1 * midpoint.Y, midpoint.X) * (180 / Math.PI)));
                            angle = angle % 360;
                        }
                        else
                        {
                            // midpoint.X is 0
                            angle = currCompass;
                        }

                        // Check if the gap found gets us closer to our destination
                        if (Math.Abs(idealDirection - angle) < bestAngle)
                        {
                            bestAngle = angle;
                            bestGap   = gapLine;
                        }
                    }
                }
            }

            return(bestGap);
        }