private void getNextTurn()
{
//Determine the robot's next turn
if (_nextAbsoluteDirection - _robotDirection == 1 || _nextAbsoluteDirection - _robotDirection == -3)
{
_nextDirective = StatusByteCode.Right;
}
else if (_nextAbsoluteDirection - _robotDirection == -1 || _nextAbsoluteDirection - _robotDirection == 3)
{
_nextDirective = StatusByteCode.Left;
}
else if (_nextAbsoluteDirection - _robotDirection == 2 || _nextAbsoluteDirection - _robotDirection == -2)
{
_nextDirective = StatusByteCode.Turn;
}
else
{
_nextDirective = StatusByteCode.Forward;
}
}
private void com_SerialDataEvent(object sender, SerialDataEventArgs e)
{
if (!_robotControlIsEnabled)
return;
_receivedByte = e.DataByte.ToStatusByteCode();
if (_receivedByte == StatusByteCode.Acknowledged)
{
if (_sentDirectiveIsUnacknowledged)
{
if (_done == true)
DisableRobotControl();
if (_nextDirective == StatusByteCode.Back)
{
Data.nav.currentPos = new NodeConnection(Data.nav.currentPos.From, true);
_halfway = true;
}
else
{
_halfway = false;
_robotDirection = _nextAbsoluteDirection;
Data.nav.currentPos = _nextNodeConnection;
}
_i++; //Advance to next item in path
_sentDirectiveIsUnacknowledged = false;
}
else
{
return;
}
}
else if (_receivedByte == StatusByteCode.NotAcknowledged)
{
//Resend directives
Data.com.SendByte((byte)_nextDirective);
}
else if (_sentDirectiveIsUnacknowledged == false)
{
if (_receivedByte == StatusByteCode.Enquiry)
{
if (_i > 0 && Data.challenge == Challenge.FindPath && (!_halfway && _nextDirective != StatusByteCode.Turn) && !Data.nav.fullPath[_i].FromPoint)
return;
if (Data.nav.fullPath.Count != 0)
{
getNextDirective();
Data.com.SendByte((byte)_nextDirective);
}
else
{
Data.com.SendByte((byte)StatusByteCode.Unknown);
}
if (Data.challenge == Challenge.FindPath)
{
if (_done == true)
Data.com.SendByte((byte)StatusByteCode.Done);
if (_continue == true)
Data.com.SendByte((byte)StatusByteCode.Continue);
_continue = false;
}
if (Data.challenge == Challenge.FindTreasure)
{
if (_done == true)
DisableRobotControl();
}
_sentDirectiveIsUnacknowledged = true;
}
else if (_receivedByte == StatusByteCode.MineDetected)
{
if (_i == 0)
return;
//Detected mine, add to list
if (Data.challenge == Challenge.FindTreasure)
{
if (!treasureSearchList.Remove(Data.nav.currentPos))
treasureSearchList.Remove(Data.nav.currentPos.Flipped);
Data.nav.visited.Add(Data.nav.currentPos);
}
Data.nav.mines.Add(Data.nav.fullPath[_i - 1]);
_i = 0;
Data.nav.recalculatePath();
Data.com.SendByte((byte)StatusByteCode.Acknowledged);
_halfway = true;
}
else if (_receivedByte == StatusByteCode.Halfway)
{
if (Data.challenge == Challenge.FindTreasure)
{
if (!treasureSearchList.Remove(Data.nav.currentPos))
treasureSearchList.Remove(Data.nav.currentPos.Flipped);
Data.nav.visited.Add(Data.nav.currentPos);
}
_halfway = true;
Data.com.SendByte((byte)StatusByteCode.Acknowledged);
}
}
else
{
//Invalid bytecode or out of sync
return;
}
Data.vis.DrawField();
Data.db.UpdateProperty("MineCount");
Data.db.UpdateProperty("CurrentPosText");
}
private void getNextDirective()
{
//Robot asks for new directions
if (Data.challenge == Challenge.FindTreasure && _i == Data.nav.fullPath.Count)
{
if (treasureSearchList.Count == 0)
{
_done = true;
_nextDirective = StatusByteCode.Done;
return;
}
_i = 0;
Data.nav.getNextAxis();
}
if (Data.nav.fullPath.Count > 0 && Data.nav.fullPath.Count > _i)
{
_nextNodeConnection = Data.nav.fullPath[_i];
}
if (((Data.nav.fullPath.Count - 1) == _i || Data.nav.fullPath.Count == 0) && Data.challenge != Challenge.FindTreasure)
{
_done = true;
}
else if (Data.nav.fullPath[_i].FromPoint == true)
{
//TargetCP visited
_nextDirective = StatusByteCode.Turn;
_nextAbsoluteDirection = (Direction)(((int)_robotDirection + 2) % 4);
if (Data.challenge == Challenge.FindPath)
{
if (Data.nav.targetCPs != null)
Data.nav.targetCPs.RemoveAt(Data.nav.currentPath);
}
return;
}
getNextDirection();
if (_robotDirection == Direction.Unknown)
GetInitialRobotDirection();
getNextTurn();
}
public void ResetRobotControl()
{
_done = false;
_halfway = false;
_robotControlIsEnabled = false;
_sentDirectiveIsUnacknowledged = false;
_nextAbsoluteDirection = Direction.Unknown; //next direction in terms of the XY grid
_robotDirection = Direction.Unknown; //current direction the robot is pointing in
_nextDirective = StatusByteCode.Unknown; //next directive to be sent to robot
_i = 0;
Data.nav.currentPath = 0;
}