Пример #1
0
        public static bool doLefetParalel()
        {
            ParallelResult rp = isLeftParalel();

            if (rp == lastParallelResult)
            {
                return(false);
            }
            lastParallelResult = rp;
            switch (rp)
            {
            case ParallelResult.Paralel:
                RealFuncs.StopEngines();
                return(true);

            case ParallelResult.PreaApropiat:
                RealFuncs.rotirePeLoc(10, 125, Engines.LeftEngines);
                break;

            case ParallelResult.Apropiat:
                RealFuncs.rotirePeLoc(10, 60, Engines.LeftEngines);
                break;

            case ParallelResult.Departat:
                RealFuncs.rotirePeLoc(10, 60, Engines.RightEngines);
                break;

            case ParallelResult.PreaDepartat:
                RealFuncs.rotirePeLoc(10, 125, Engines.RightEngines);
                break;
            }
            return(false);
        }
Пример #2
0
        public static bool doLeftDistance(double dist, double epsilon = 10)
        {
            double distLeft = RealFuncs.getSensorValue(Sensor.SideLeft);

            switch (drdState)
            {
            case 0:
                if (Math.Abs(distLeft - dist) < epsilon)
                {
                    return(true);
                }
                angle1  = (int)(Math.Abs(distLeft - dist)) / 1.5;
                drdTmp1 = (distLeft + funcs.wpfPixelsToCMs(ext.TheCar.ActualWidth / 2)) / Math.Cos(angle1 * Math.PI / 180);
                if (distLeft > dist)
                {
                    drdpr = ParallelResult.PreaDepartat;
                }
                else
                {
                    drdpr = ParallelResult.PreaApropiat;
                }
                RealFuncs.rotirePeLoc(30, 60, Engines.LeftEngines);
                drdState = 1;
                break;

            case 1:
                //funcs.Log("drdtmp: " + drdTmp1.Round(2).ToString() + "   dr:" + distRight.Round().ToString());
                if ((distLeft + funcs.wpfPixelsToCMs(ext.TheCar.ActualWidth / 2)) > drdTmp1)
                {
                    RealFuncs.StopEngines();
                    drdState = 2;
                }
                break;

            case 2:
                drdTmp1 = (dist + funcs.wpfPixelsToCMs(ext.TheCar.ActualWidth / 2)) / Math.Cos(angle1 * Math.PI / 180);
                distantaParcursa(true);
                if (drdpr == ParallelResult.PreaDepartat)
                {
                    RealFuncs.goBack(5, 60); drdState = 3;
                }
                else
                {
                    RealFuncs.goFront(5, 60); drdState = 4;
                }
                //funcs.Log("drdtmp:" + drdTmp1.ToString());
                break;

            case 3:
                if (distLeft + funcs.wpfPixelsToCMs(ext.TheCar.ActualWidth / 2) < drdTmp1)
                {
                    RealFuncs.StopEngines();
                    drdState = 5;
                    dp1      = distantaParcursa();
                }
                break;

            case 4:
                if (distLeft + funcs.wpfPixelsToCMs(ext.TheCar.ActualWidth / 2) > drdTmp1)
                {
                    RealFuncs.StopEngines();
                    drdState = 5;
                    dp1      = distantaParcursa();
                }
                break;

            case 5:
                if (doLefetParalel())
                {
                    if (drdpr == ParallelResult.PreaApropiat)
                    {
                        RealFuncs.goBack(10, 60); drdState = 6;
                    }
                    else
                    {
                        RealFuncs.goFront(10, 60); drdState = 6;
                    }
                    distantaParcursa(true);
                }
                break;

            case 6:
                if (distantaParcursa() > dp1 * Math.Cos(angle1 * Math.PI / 180))
                {
                    RealFuncs.StopEngines();
                    //funcs.Log("cw: " + funcs.wpfPixelsToCMs(ext.TheCar.ActualWidth).ToString());
                    return(true);
                }
                break;
            }


            return(false);
        }