コード例 #1
0
        public bool IsComplete()
        {
            short ascentHeading = AscentPacketHandler.Compass;

            GPS    ascent        = AscentPacketHandler.GPSData;
            double headingToGate = ascent.GetHeadingTo(this.gate);

            return(isScanning && isScanNearlyComplete && IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, HEADING_THRESHOLD) && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() > scanStartTimestampMillis + 2000));
        }
コード例 #2
0
        public DriveCommand FindNextDriveCommand()
        {
            short ascentHeading = AscentPacketHandler.Compass;

            if (this.isAligningToHeading)
            {
                // Turn toward heading
                // Scan, use heading as reference

                GPS    ascent        = AscentPacketHandler.GPSData;
                double headingToGate = ascent.GetHeadingTo(this.gate);

                Console.Write("Scan aligning toward heading | compass: "******" | Heading to gate: " + headingToGate + " | ");



                // Have reached heading. Start turning right
                if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, HEADING_THRESHOLD))
                {
                    this.isAligningToHeading = false;

                    // If driving for a certain duration
                    if (driveTowardHeadingForMillis > 0)
                    {
                        // Drive straight for a duration
                        this.isDrivingStraightForDuration = true;
                        this.driveStraightUntilMillis     = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() + this.driveTowardHeadingForMillis;

                        return(DriveCommand.Straight(Speed.VISION_SCAN));
                    }
                    else
                    {
                        // Start scan
                        this.isScanning          = true;
                        scanStartHeading         = ascentHeading;
                        scanStartTimestampMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                        return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                    }
                }

                // Turn toward heading angle
                if (ascentHeading < ((headingToGate + 180) % 360) && ascentHeading > headingToGate)
                {
                    return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
                }
                else
                {
                    return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                }
            }
            else if (this.isDrivingStraightForDuration)
            {
                if (this.driveStraightUntilMillis > DateTimeOffset.UtcNow.ToUnixTimeMilliseconds())
                {
                    // Drive straight for duration
                    return(DriveCommand.Straight(Speed.VISION_SCAN));
                }
                else
                {
                    // Start scan
                    this.isDrivingStraightForDuration = false;

                    this.isScanning          = true;
                    scanStartHeading         = ascentHeading;
                    scanStartTimestampMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                    return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                }
            }
            else
            {
                // Scan
                // ... more to do for this case
                Console.Write("Scan scanning | ");

                // Increment deltaTheta accordingly
                deltaTheta = (ascentHeading - scanStartHeading + 360) % 360;

                Console.Write("Scan scanning | compass: "******" | deltaTheta: " + deltaTheta + " | ");

                if (deltaTheta > 330 && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() > scanStartTimestampMillis + 2000))
                {
                    // Use boolean incase of wrap around to 0 due to mod
                    isScanNearlyComplete = true;
                }

                return(DriveCommand.RightTurn(Speed.VISION_SCAN));
            }
        }