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); }
public static bool parcareLaterala(QueueEntry qe, EventArgs e) { funcs.Log("st=" + state2.ToString()); switch (state2) { case 0: RealFuncs.goFront(30, 200); state2 = 1; break; case 1: tmp2 = funcs.getSensorValue(Sensor.SideRight); if (tmp2 < 50) { state2 = 2; } break; case 2: tmp2 = funcs.getSensorValue(Sensor.SideRight); if (tmp2 > 50) { state2 = 3; RealFuncs.goFrontRight(30, 200); } break; case 3: tmp2 = funcs.getSensorValue(Sensor.FrontRight); if (tmp2 < 55) { state2 = 4; RealFuncs.goFrontLeft(30, 250); } break; case 4: if (isRightParalel(3) == 0) { RealFuncs.goFront(30, 175); state2 = 5; return(false); } break; case 5: if (Math.Abs(funcs.getSensorValue(Sensor.FrontLeft) - funcs.getSensorValue(Sensor.FrontRight)) < 10) { RealFuncs.StopEngines(); RealFuncs.goBack(1, 150); return(true); } break; case 6: break; default: break; } return(false); }