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