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);
}
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);
}