public override IPAction ExecuteRest(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
int currentDistance = repSensors.SensorValueInt("Encoder1");
if (currentDistance == m_RequestedDistance)
{
// All done, move to next operator
m_RequestedDistance = m_DesiredDistance;
return IPAction.Increment;
}
else if (repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Halt && currentDistance > 0)
{
// Something stopped us so start over and request the remaining distance
m_RequestedDistance = m_DesiredDistance - currentDistance;
return IPAction.DoFirst;
}
else if (repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Fwd && m_DesiredHeading != -1)
{
// Heading hold
int herr = HeadingError(repSensors.SensorValueInt("Heading"));
int corr = m_HhPID.Calculate(herr);
if (corr > 0)
requests.Enqueue(new Request() { Name = "Right Turn" + corr.ToString(), Channel = "Drive", Command = "RX" + corr.ToString() });
else if (corr < 0)
requests.Enqueue(new Request() { Name = "Left Turn" + Math.Abs(corr).ToString(), Channel = "Drive", Command = "LX" + Math.Abs(corr).ToString() });
}
return IPAction.DoRest;
}
public override RequestQueue Execute(SensorRepository repSensors, RequestQueue LastWinner)
{
bool iWon = LastWinner != null && LastWinner.BehaviorName == m_Name;
RequestQueue requests = new RequestQueue(m_Name);
// Ignore if powered off or during a spin
if (!repSensors.SensorValueBool("IsPower") ||
repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_SpinCW ||
repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_SpinCCW)
return requests;
if (repSensors.SensorValueInt("DistFwd") <= 8)
{
if (iWon || repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Halt)
{
// Halt already sent so create a NOP request to retain control
requests.Enqueue(new Request() { Name = "NOP", Channel = "NA", Command = "NP" });
}
else
{
// Halt
requests.Enqueue(new Request() { Name = "Halt", Channel = "Drive", Command = "HL" });
}
}
return requests;
}
public override IPAction ExecuteRest(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
if (repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Halt)
return IPAction.Increment;
else
return IPAction.DoRest;
}
public override RequestQueue Execute(SensorRepository repSensors, RequestQueue LastWinner)
{
bool iWon = LastWinner != null && LastWinner.BehaviorName == m_Name;
RequestQueue requests = new RequestQueue(m_Name);
if (repSensors.SensorValueBool("IsPower") && repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Fwd)
{
if(repSensors.SensorValueInt("DistL30") < repSensors.SensorValueInt("DistR30"))
{
if(repSensors.SensorValueInt("DistL30") < 20 || (iWon && repSensors.SensorValueInt("DistL30") < 24))
{
requests.Enqueue(new Request() { Name = "Turn Right", Channel = "Drive", Command = "RT" });
}
}
else
{
if(repSensors.SensorValueInt("DistR30") < 20 || (iWon && repSensors.SensorValueInt("DistR30") < 24))
{
requests.Enqueue(new Request() { Name = "Turn Left", Channel = "Drive", Command = "LF" });
}
}
}
return requests;
}
public override RequestQueue Execute(SensorRepository repSensors, RequestQueue LastWinner)
{
bool iWon = LastWinner != null && LastWinner.BehaviorName == m_Name;
RequestQueue requests = new RequestQueue(m_Name);
if (repSensors.SensorValueBool("IsPower") && repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Fwd && m_DesiredHeading != -1)
{
int herr = HeadingError(repSensors.SensorValueInt("Heading"));
int corr = m_HhPID.Calculate(herr);
if(corr > 0)
requests.Enqueue(new Request() { Name = "Right Turn" + corr.ToString(), Channel = "Drive", Command = "RX" + corr.ToString() });
else if (corr < 0)
requests.Enqueue(new Request() { Name = "Left Turn" + Math.Abs(corr).ToString(), Channel = "Drive", Command = "LX" + Math.Abs(corr).ToString() });
}
return requests;
}
public override RequestQueue Execute(SensorRepository repSensors, RequestQueue LastWinner)
{
bool iWon = LastWinner != null && LastWinner.BehaviorName == m_Name;
RequestQueue requests = new RequestQueue(m_Name);
if (!iWon && repSensors.SensorValueInt("Direction") == (int)Cruise.MoveDir.Mov_Halt)
{
requests.Enqueue(new Request() { Name = "Halt/Reset", Channel = "Drive", Command = "EH" });
requests.Enqueue(new Request() { Name = "Fwd " + m_CruiseDistance, Channel = "Drive", Command = "FX" + m_CruiseDistance });
}
return requests;
}
public override IPAction ExecuteFirst(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
if (!iWon)
{
requests.Enqueue(new Request() { Name = "Halt/Reset", Channel = "Drive", Command = "EH" });
// Calculate how many degrees to spin to get to desired heading
int herr = HeadingError(repSensors.SensorValueInt("Heading"));
m_RequestedCorrection = Math.Abs(herr);
if (herr > 0)
requests.Enqueue(new Request() { Name = "SpinCCW " + m_RequestedCorrection.ToString(), Channel = "Drive", Command = "LX" + m_RequestedCorrection.ToString() });
else if (herr < 0)
requests.Enqueue(new Request() { Name = "SpinCW " + m_RequestedCorrection.ToString(), Channel = "Drive", Command = "RX" + m_RequestedCorrection.ToString() });
else
return IPAction.Increment;
return IPAction.DoFirst;
}
else
return IPAction.DoRest;
}
