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 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);
// 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 NavOperator.IPAction ExecuteFirst(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
if (!iWon)
{
requests.Enqueue(new Request() { Name = "Set Speed" + m_Speed.ToString(), Channel = "Drive", Command = "S" + m_Speed.ToString() });
return IPAction.DoFirst;
}
else
return IPAction.Increment;
}
public override IPAction ExecuteFirst(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
if (!iWon)
{
requests.Enqueue(new Request() { Name = "Halt/Reset", Channel = "Drive", Command = "EH" });
requests.Enqueue(new Request() { Name = "Fwd " + m_RequestedDistance, Channel = "Drive", Command = "FX" + m_RequestedDistance });
return IPAction.DoFirst;
}
else
return IPAction.DoRest;
}
public override NavOperator.IPAction ExecuteFirst(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
if (!iWon)
{
// Halt
requests.Enqueue(new Request() { Name = "Halt", Channel = "Drive", Command = "HL" });
return IPAction.DoFirst;
}
else
return IPAction.Increment;
}
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 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 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;
}
public override RequestQueue Execute(SensorRepository repSensors, RequestQueue LastWinner)
{
bool iWon = LastWinner != null && LastWinner.BehaviorName == m_Name;
RequestQueue requests = new RequestQueue(m_Name);
if (m_InstPtr < m_Route.Count)
{
NavOperator.IPAction action;
if (m_IsFirst)
action = m_Route[m_InstPtr].ExecuteFirst(repSensors, requests, iWon);
else
action = m_Route[m_InstPtr].ExecuteRest(repSensors, requests, iWon);
switch (action)
{
case NavOperator.IPAction.Increment:
m_InstPtr++;
m_IsFirst = true;
break;
case NavOperator.IPAction.Reset:
m_InstPtr = 0;
m_IsFirst = true;
break;
case NavOperator.IPAction.DoFirst:
m_IsFirst = true;
break;
case NavOperator.IPAction.DoRest:
m_IsFirst = false;
break;
}
}
return requests;
}
private void ReceiveSensorDataTask(RobotSpecification specRobot)
{
while (true)
{
if (m_tsCancel.IsCancellationRequested)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor Task stopped"));
break;
}
else
{
if (m_RunSensors)
{
try
{
// Time telemetry request/reception
System.Diagnostics.Stopwatch sw = System.Diagnostics.Stopwatch.StartNew();
// Request and receive remote sensor data via telemetry channel
SensorRepository sr = new SensorRepository(specRobot);
m_Channels[m_TelemetryChannel].Send("T1");
string data = m_Channels[m_TelemetryChannel].ReadLine();
sw.Stop();
// Parse received sensor data
sr.ParseSensorData(data, specRobot.SensorSpecs.Count);
if (sr.IsValid)
{
// Add sensor repository to behavior/arbitrate queue
if (m_SensorReadings.TryAdd(sr, 1, m_tsCancel.Token))
{
sr.AcquisitionTime = sw.ElapsedMilliseconds;
sr.ID = m_SensorRepID;
m_SensorRepID++;
}
else
{
// Behaviors have not consumed previous sensor data so remove it and get newer
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor data not used."));
if (m_SensorReadings.TryTake(out sr, 1, m_tsCancel.Token))
m_LogEntries.AddEntry(new ActivityLogEntry("Old sensor data removed!"));
}
}
else
{
// Lost communications with the robot
m_LogEntries.AddEntry(new ActivityLogEntry(ActivityLogEntry.LogEntryType.Error, "No communications with the robot!", null));
m_LogEntries.AddEntry(new ActivityLogEntry("RX Data = " + data));
}
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor Acquisition Time", sw.ElapsedMilliseconds));
}
catch (Exception ex)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Exception in Sensor Task: " + ex.Message));
}
}
}
}
}
public abstract IPAction ExecuteRest(SensorRepository repSensors, RequestQueue requests, bool iWon);
public override IPAction ExecuteRest(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
throw new NotImplementedException();
}
public override IPAction ExecuteFirst(SensorRepository repSensors, RequestQueue requests, bool iWon)
{
return IPAction.Reset;
}
public abstract RequestQueue Execute(SensorRepository repSensors, RequestQueue LastWinner);
