public void Suche()
{
while (true)
{
ActivCheck(WiringPiLib.DigitalRead(ACTIVPIN) == 1);
if (!IsActive)
{
Log4Net.Info($"INACTIVE, Alte Position: {LenkServo.Position}");
Thread.Sleep(300);
continue;
}
if (WiringPiLib.DigitalRead(SHUTDOWNPIN) == 0)
{
Process.Start("/usr/bin/sudo", "/sbin/shutdown -h now");
}
long sensorResult = ReadSensors();
if (SensorsPlausible(sensorResult))
{
Lenke(GetSollDirection(sensorResult));
}
else if (TooLongUnplausibleSensorResult)
{
Log4Net.Info("Zu lange unplausibel");
LenkServo.SetPosition(0);
}
Thread.Sleep(SleepBetweenActions);
}
}
private void Lenke(Direction direction)
{
Log4Net.Info($"Lenke: {direction}, Alte Position: {LenkServo.Position}");
if (direction == Direction.OnTrack)
{
//Auf der perfekten Linie nix machen
LenkServo.SetPosition(0);
}
if (direction == Direction.GoLeft)
{
if (LenkServo.Position > 0) //Wir sollen nach links, fahren aber noch rechts: Trägheit
{
LenkServo.SetPosition(0);
}
else if (LenkServo.Position > (LenkDeltaInGrad - 90))
{
LenkServo.SetPosition(LenkServo.Position - LenkDeltaInGrad);
}
}
if (direction == Direction.GoRight)
{
if (LenkServo.Position < 0)
{
LenkServo.SetPosition(0); //Trägheit
}
else if (LenkServo.Position < (90 + LenkDeltaInGrad))
{
LenkServo.SetPosition(LenkServo.Position + LenkDeltaInGrad);
}
}
}
void ActivCheck(bool sollState)
{
if (IsActive == sollState)
{
return;
}
Thread.Sleep(300); //Entprellen
IsActive = sollState;
if (!IsActive)
{
LenkServo.SetPosition(0);
}
}
public void ServoTest()
{
// while (true)
// {
// LenkServo.SetPosition (0);
// LenkServo.SetPosition (5);
// LenkServo.SetPosition (10);
// LenkServo.SetPosition (15);
// LenkServo.SetPosition (20);
// LenkServo.SetPosition (25);
// LenkServo.SetPosition (30);
// LenkServo.SetPosition (35);
// LenkServo.SetPosition (40);
// LenkServo.SetPosition (45);
// LenkServo.SetPosition (50);
// LenkServo.SetPosition (55);
// LenkServo.SetPosition (60);
// LenkServo.SetPosition (65);
// LenkServo.SetPosition (70);
// LenkServo.SetPosition (75);
// LenkServo.SetPosition (80);
// LenkServo.SetPosition (85);
// LenkServo.SetPosition (90);
// }
LenkServo.SetPosition(-10);
LenkServo.SetPosition(-30);
while (true)
{
for (int i = -45; i < 45; i++)
{
LenkServo.SetPosition(i);
Thread.Sleep(40);
}
for (int i = 45; i > -45; i--)
{
LenkServo.SetPosition(i);
Thread.Sleep(40);
}
}
}