public static void Payload(String newPayload)
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
if (f.HasPallet)
{
PrintError("Forklift is already carrying pallet: '" + f.Payload.Name + "'");
return;
}
Pallet pallet = null;
foreach (KeyValuePair <int, Pallet> palletPair in Database.Instance.Data.Pallets)
{
if (String.Equals(palletPair.Value.Name, newPayload, StringComparison.CurrentCultureIgnoreCase))
{
pallet = palletPair.Value;
break;
}
}
if (pallet == null)
{
PrintError("Could not find pallet with name '" + newPayload + "'");
return;
}
Database.Instance.Data.Forklifts.FirstOrDefault().Value.Payload = pallet;
PrintSuccess("Payload updated");
}
public static void Palletlist(string subcommand, string name)
{
if (subcommand == SUBCOMMAND_REMOVE)
{
foreach (KeyValuePair <int, Pallet> palletPair in Database.Instance.Data.Pallets)
{
if (String.Equals(palletPair.Value.Name, name, StringComparison.CurrentCultureIgnoreCase))
{
Database.Instance.Data.RemovePallet(palletPair.Value);
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
// Remove the payload fron the forklift
if (f.Payload.Equals(palletPair.Value))
{
f.Payload = null;
}
PrintSuccess("Pallet removed");
return;
}
}
PrintError("Could not find pallet with name '" + name + "'");
}
else
{
PrintIncorrectUse();
PalletlistHelp();
}
}
public static void Position(string frontNode, string rearNode)
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node front = null;
Node rear = null;
try
{
front = g.getNode(frontNode);
rear = g.getNode(rearNode);
}
catch (NodeException e)
{
PrintError(e.Message);
return;
}
try
{
f.UpdateNodes(front, rear);
}
catch (NodeException e)
{
PrintError(e.Message);
return;
}
PrintSuccess("Position updated");
Position();
}
public override byte[] GetBytes()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node ignore = f.RearNode;
ForkliftPath fp = new ForkliftPath(GetPath(), ignore);
return(Encoding.ASCII.GetBytes(fp.getDirections()));
}
// Populates the database with the forklift
private static void PopulateDatabaseWithForklift()
{
if (Database.Instance.Data.Forklifts.Count <= 0)
{
Forklift forklift = new Forklift();
Database.Instance.Data.AddForklift(forklift);
}
}
public void AddForklift(Forklift forklift)
{
try
{
Forklifts.Add(forklift.Identifier, forklift);
}
catch (ArgumentException)
{
Forklifts.Remove(forklift.Identifier);
Forklifts.Add(forklift.Identifier, forklift);
}
}
public static void Payload()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
if (f.HasPallet)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Payload: '" + f.Payload.Name + "'");
}
else
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "No payload");
}
}
public static void Position()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
if (f.FrontNode == null || f.RearNode == null)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Position: Unknown");
}
else
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Front-node: " + f.FrontNode.Name);
ConsoleHandler.AddMessage(MessageType.REGULAR, "Rear-node: " + f.RearNode.Name);
}
}
public override byte[] GetBytes()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node ignore = f.RearNode;
Path p = GetPath();
if (p == null)
{
throw new NullReferenceException("Path is empty (either at destination, or impossible)");
}
ForkliftPath fp = new ForkliftPath(p, ignore);
string directions = fp.getDirections();
// We add some special instructions to handle the different job types
// D - forklift down
// N - move to next red tape
// U - forklift up
// T - Turn around
// B - Back off small step
// The instructions is inserted in reversed order
if (Type == PalletJobType.fetch)
{
// Removes the extra N
directions = directions.Substring(1, directions.Length - 1);
// Add directions to fetch the pallet
directions = directions.Insert(0, "TUNDR");
}
else if (Type == PalletJobType.deliver)
{
// Removes the extra N
directions = directions.Substring(1, directions.Length - 1);
// Add directions to deliver the pallet
directions = directions.Insert(0, "TUBDNR");
}
return(Encoding.ASCII.GetBytes(directions));
}
// Used to convert status to byte
public char GetStatusByte(Forklift f)
{
switch (f.Status)
{
case Status.IDLE:
return(STATUS_IDLE);
case Status.BUSY:
return(STATUS_BUSY);
case Status.ERROR:
return(STATUS_ERROR);
case Status.OBSTACLE:
return(STATUS_OBSTACLE);
default:
return(STATUS_UNKNOWN);
}
}
public override Path GetPath()
{
if (Destination != null)
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node from = f.FrontNode;
Node ignore = f.RearNode;
Path p = g.ShortestPath(from, Destination, ignore);
if (p.Nodes.Last().Equals(f.FrontNode))
{
throw new JobException("Already at destination '" + f.FrontNode.Name + "'");
}
return(p);
}
// Neither directions or destination is set, cannot generate path
return(null);
}
public bool RemoveForklift(Forklift forklift)
{
bool result = Forklifts.Remove(forklift.Identifier);
return result;
}
public void AddForklift(Forklift forklift)
{
try
{
Forklifts.Add(forklift.Identifier, forklift);
}
catch (ArgumentException)
{
Forklifts.Remove(forklift.Identifier);
Forklifts.Add(forklift.Identifier, forklift);
}
}
public bool RemoveForklift(Forklift forklift)
{
bool result = Forklifts.Remove(forklift.Identifier);
return(result);
}
// Used to convert status to byte
public char GetStatusByte(Forklift f)
{
switch (f.Status)
{
case Status.IDLE:
return STATUS_IDLE;
case Status.BUSY:
return STATUS_BUSY;
case Status.ERROR:
return STATUS_ERROR;
case Status.OBSTACLE:
return STATUS_OBSTACLE;
default :
return STATUS_UNKNOWN;
}
}
// Used to check if position information is required
public static void UpdatePositionInformation()
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
if (f.RearNode == null || f.FrontNode == null)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Information about PALL-E position is required.");
bool result;
do
{
Node rearNode = null;
Node frontNode = null;
// The user must enter a faceing node
while (frontNode == null)
{
try
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Enter name of front node...");
String faceing = Console.ReadLine();
ConsoleHandler.ClearCommand();
frontNode = g.getNode(faceing);
}
catch (NodeException e)
{
ConsoleHandler.AddMessage(MessageType.ERROR, "Error when finding node: '" + e.Message + "'");
}
}
// The user must enter a last node
while (rearNode == null)
{
try
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Enter name of rear node...");
String last = Console.ReadLine();
ConsoleHandler.ClearCommand();
rearNode = g.getNode(last);
}
catch (NodeException e)
{
ConsoleHandler.AddMessage(MessageType.ERROR, "Error when finding node: '" + e.Message + "'");
}
}
// Update node information
result = f.UpdateNodes(frontNode, rearNode);
Commands.PrintSuccess("Position updated");
Commands.Position();
}while (!result);
}
else
{
ConsoleHandler.AddMessage(MessageType.REGULAR, ConsoleHandler.DNS + " position information:");
ConsoleHandler.AddMessage(MessageType.REGULAR, "Front-node: '" + f.FrontNode.Name + "'. Rear-node: '" + f.RearNode.Name + "'");
}
}
// Consumes one package
private void ConsumePackage(byte[] package)
{
// Allocates data from the package
byte[] data = new byte[package.Length - 3];
// Finds the type of pakcage
byte byteType = package[1];
// Fills the data array with the corresponding data
for (int i = 2, j = 0; i < package.Length - 1; i++, j++)
{
data[j] = package[i];
}
// Convert the data to a string
string dataString = Encoding.UTF8.GetString(data, 0, data.Length);
// Find the correct method to consume the package
switch (byteType)
{
case TYPE_UPDATE_COLOR:
// Tell the user that the color is being fetched
// dataString contains the id of the color
ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Sending color #" + dataString + "...");
// Get the color from the database
Color requestedColor = Database.Instance.Data.Colors.FirstOrDefault(i => i.Value.Identifier == int.Parse(dataString)).Value;
// Convert the RGB-values of the color to byte array
byte[] returnData = requestedColor.ToRGBBytes();
// Send the package back to the NXT
SendPackageBT(TYPE_FETCHED_COLOR, returnData);
break;
case TYPE_SAVE_COLOR:
// Split the data into to values that needs to be saved
int colorId = int.Parse(dataString.Substring(0, 3)); // Use the first char as color identifer
int red = int.Parse(dataString.Substring(3, 3)); // Use the first char as red value
int green = int.Parse(dataString.Substring(6, 3)); // Use the first char as green value
int blue = int.Parse(dataString.Substring(9, 3)); // Use the first char as blue value
// Tell the user that the color is being saved
ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Received color #" + colorId + ". (" + red + ", " + green + ", " + blue + ")");
// Find the old color
Color color = Database.Instance.Data.Colors.FirstOrDefault(i => i.Value.Identifier == colorId).Value;
// Update the values
color.Red = red;
color.Green = green;
color.Blue = blue;
// Save the updated color
Database.Instance.Data.AddColor(color);
break;
case TYPE_REPORT_OBSTACLE:
ConsoleHandler.AddMessage(MessageType.ERROR, "Obstacle encountered!");
int directionsIndex = int.Parse(dataString);
if (!(CurrentJob is TurnJob) && !(CurrentJob is DebugJob))
{
if (CurrentJob != null)
{
Path currentPath = CurrentJob.GetPath();
String directions = new ForkliftPath(currentPath, forklift.RearNode).getDirections();
if (CurrentJob is PalletJob)
{
PalletJob pj = (PalletJob)CurrentJob;
if (pj.Type == PalletJobType.fetch)
{
directions = directions.Insert(0, "TUND");
}
else if (pj.Type == PalletJobType.deliver)
{
directions = directions.Insert(0, "TUBDN");
}
}
int max = directions.Length;
int i = directionsIndex;
if (directions[directions.Length - 1] == 'L' || directions[directions.Length - 1] == 'R')
{
max--;
}
double nodeRearIndex = (max - 1.0 - i) / 2.0;
if (Math.Abs(nodeRearIndex % 1 - 0.5) < 0.4)
{
ConsoleHandler.AddMessage(MessageType.ERROR, "Currently inside intersection - aborting all jobs");
forklift.FrontNode = null;
forklift.FrontNode = null;
// Inform the user to update the position
ConsoleHandler.AddMessage(MessageType.ERROR, "Please update the position of the forklift...");
while (forklift.FrontNode == null || forklift.RearNode == null)
{
;
}
}
else
{
Node newRearNode = null;
Node newFrontNode = null;
int roundedIndex = (int)nodeRearIndex;
if (roundedIndex == 0)
{
newFrontNode = forklift.FrontNode;
newRearNode = forklift.RearNode;
}
else
{
newFrontNode = currentPath.Nodes[roundedIndex];
newRearNode = currentPath.Nodes[roundedIndex - 1];
}
// Update the visitied count on edges
Node previousNode = null;
foreach (Node n in currentPath.Nodes)
{
if (previousNode == null)
{
previousNode = n;
continue;
}
// Find edge to the previous node and increment the visitied count
KeyValuePair <Node, Edge> nodeEdgePair = previousNode.Neighbours.Single(x => x.Key != null && x.Key.Equals(n));
nodeEdgePair.Value.Visited++;
if (n.Equals(newFrontNode))
{
break;
}
previousNode = n;
}
// Update forklift nodes
forklift.FrontNode = newFrontNode;
forklift.RearNode = newRearNode;
// Update edge the NXT is standing on
Database.Instance.Data.Graphs.FirstOrDefault().Value.BlockEdge(newFrontNode, newRearNode);
}
}
else
{
Commands.PrintError("No current job - Continuing...");
}
}
else if (CurrentJob is TurnJob)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Already performing turn-job... (ignoreing)");
}
else if (CurrentJob is DebugJob)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Performing debug-job... (ignoreing debugjobs)");
}
break;
// Check if the NXT updated its status
case TYPE_UPDATE_STATUS:
{
if (dataString[0] != GetStatusByte(forklift))
{
// Tell the user what the status the NXT updated to
ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Received " + ConsoleHandler.DNS + "-status: " + dataString);
}
// Check what status the NXT told us
switch (dataString[0])
{
// The NXT was idle
case STATUS_IDLE:
// Check if the nxt just completed a job
if (forklift.Status == Status.BUSY && CurrentJob != null)
{
Path p = CurrentJob.GetPath();
// Update the visited count on traversed edges
Node previousNode = null;
foreach (Node n in p.Nodes)
{
if (previousNode == null)
{
previousNode = n;
continue;
}
// Find edge to the previous node and increment the visitied count
KeyValuePair <Node, Edge> nodeEdgePair = previousNode.Neighbours.Single(x => x.Key != null && x.Key.Equals(n));
nodeEdgePair.Value.Visited++;
previousNode = n;
}
// Update the position of the forklift
if (p.Nodes.Count >= 2)
{
Node frontNode = p.Nodes.ElementAt(p.Nodes.Count - 1);
Node rearNode = p.Nodes.ElementAt(p.Nodes.Count - 2);
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
f.UpdateNodes(frontNode, rearNode);
}
if (!(CurrentJob is TurnJob))
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
// Unblock all edges
for (int i = g.BlockedEdges.Count - 1; i >= 0; i--)
{
Tuple <Node, Node> edge = g.BlockedEdges[i];
g.UnblockEdge(edge.Item1, edge.Item2);
}
// Decay the graph
g.Decay(0.90);
}
if (CurrentJob is PalletJob)
{
PalletJob job = (PalletJob)CurrentJob;
// Check if the forklift just finished a deliver pallet job
if (job.Type == PalletJobType.deliver)
{
Pallet pallet = forklift.Payload;
// Update the location of the pallet
pallet.Location = p.Nodes.Last();
// Update the payload of the forklift
forklift.Payload = null;
}
else if (job.Type == PalletJobType.fetch)
{
Node n = p.Nodes.Last();
// Update the payload (Which will also update the location of the pallet)
forklift.Payload = n.Pallet;
}
// Swap front and rear node
forklift.UpdateNodes(forklift.RearNode, forklift.FrontNode);
}
CurrentJob = null;
}
// Update the internal status
forklift.Status = Status.IDLE;
// Check if PALL-E should get another job
if (CurrentJob == null && forklift.Status == Status.IDLE)
{
Job nextJob = null;
while ((nextJob == null || nextJob.GetPath() != null) && JobList.Count > 0)
{
// Get the next job (job with lowest id)
nextJob = JobList.Aggregate((l, r) => l.Key < r.Key ? l : r).Value;
try
{
nextJob.GetPath();
break;
}
catch (PathException e)
{
Commands.PrintError("Remove job #" + nextJob.ID() + ": '" + e.Message + "'");
}
catch (JobException e)
{
Commands.PrintError("Remove job #" + nextJob.ID() + ": '" + e.Message + "'");
}
Database.Instance.Data.RemoveJob(nextJob);
nextJob = null;
}
// We removed all the jobs, nothing left to do!
if (JobList.Count < 1)
{
break;
}
try
{
// Test if the path can be generated
nextJob.GetPath();
// Tell the user what job was sent
Commands.PrintSuccess("Sending Job #" + nextJob.ID() + " to PALL-E. " + (JobList.Count - 1) + " jobs left");
// Send the job to the NXT
SendPackageBT(nextJob.GetJobTypeBytes(), nextJob.GetBytes());
}
catch (JobException e)
{
Commands.PrintError("Job #" + nextJob.ID() + " cancelled: '" + e.Message + "'");
Database.Instance.Data.RemoveJob(nextJob);
}
}
break;
// The NXT was busy
case STATUS_BUSY:
// If the NXT was just idle, you know you have given it a job
if (forklift.Status == Status.IDLE)
{
// Debug jobs has highe priority
if (JobList.Count > 0)
{
// Remove the job that is being executed currently
CurrentJob = JobList.Aggregate((l, r) => l.Key < r.Key ? l : r).Value;
Database.Instance.Data.Jobs.Remove(CurrentJob.Identifier);
}
}
// Update the internal status
forklift.Status = Status.BUSY;
break;
case STATUS_OBSTACLE:
if (CurrentJob != null)
{
if (!(CurrentJob is DebugJob))
{
if (!(CurrentJob is TurnJob))
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Calculating alternative path...");
Database.Instance.Data.AddJob(CurrentJob);
CurrentJob = new TurnJob(Database.Instance.Data.Jobs.Keys.Min() - 1);
Node oldFrontNode = forklift.FrontNode;
Node oldRearNode = forklift.RearNode;
forklift.FrontNode = oldRearNode;
forklift.RearNode = oldFrontNode;
ConsoleHandler.AddMessage(MessageType.REGULAR, "Sending turn-job to PALL-E...");
}
else
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Resending turn-job to " + ConsoleHandler.DNS + "...");
}
// Send turn job
SendPackageBT(CurrentJob.GetJobTypeBytes(), CurrentJob.GetBytes());
}
else
{
// Send empty package (so that obstacle status will be avoided)
DebugJob dj = new DebugJob(-1, "");
SendPackageBT(dj.GetJobTypeBytes(), dj.GetBytes());
}
}
else
{
ConsoleHandler.AddMessage(MessageType.ERROR, "Tried to recover from obstacle, but there are no jobs left");
// throw new Exception("No current jobs");
}
// The status should not be updated if the current job is a debugjob
if (!(CurrentJob is DebugJob))
{
// Update the internal status
forklift.Status = Status.OBSTACLE;
}
break;
// The NXT encoutered an error
case STATUS_ERROR:
// Update the internal status
forklift.Status = Status.ERROR;
// Tell the user that the NXT encountered an error
ConsoleHandler.AddMessage(MessageType.ERROR, ConsoleHandler.DNS + " reported an error");
break;
default:
forklift.Status = Status.UNKNOWN;
break;
}
break;
}
}
return;
}
// Populates the database with the forklift
private static void PopulateDatabaseWithForklift()
{
if (Database.Instance.Data.Forklifts.Count <= 0)
{
Forklift forklift = new Forklift();
Database.Instance.Data.AddForklift(forklift);
}
}
public static void Palletlist(string subcommand, string name, string location)
{
if (subcommand == SUBCOMMAND_ADD)
{
if (location == "forklift")
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
if (f.HasPallet)
{
PrintError("Forklift is already carrying pallet: '" + f.Payload.Name + "'");
return;
}
try
{
Pallet p = new Pallet(Database.Instance.Data.GetNewPalletIdentifier(), name.ToUpper(), null);
Database.Instance.Data.AddPallet(p);
f.Payload = p;
PrintSuccess("Pallet added.");
}
catch (PalletException e)
{
PrintError(e.Message);
return;
}
}
else
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
try
{
Node n = g.getNode(location);
if (!n.IsPalletNode)
{
PrintError("Node '" + n.Name + "' is not a pallet-node.");
return;
}
if (n.HasPallet)
{
PrintError("Node '" + n.Name + "' is already containing a pallet.");
return;
}
try
{
Pallet p = new Pallet(Database.Instance.Data.GetNewPalletIdentifier(), name.ToUpper(), n);
Database.Instance.Data.AddPallet(p);
PrintSuccess("Pallet added.");
}
catch (PalletException e)
{
PrintError(e.Message);
return;
}
}
catch (NodeException e)
{
PrintError(e.Message);
return;
}
}
}
else
{
PrintIncorrectUse();
PalletlistHelp();
}
}
public override Path GetPath()
{
Node target = null;
if (Destination != null)
{
target = Destination;
}
else if (PalletLocation != null)
{
target = PalletLocation.Location;
}
else
{
throw new NodeException("Cannot get path because of missing destination or pallet location (both are null)");
}
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node from = f.FrontNode;
Node ignore = f.RearNode;
Path p = g.ShortestPath(from, target, ignore);
if (Type == PalletJobType.fetch)
{
// Cannot fetch pallets with payload
if (f.HasPallet)
{
throw new JobException("Cannot fetch pallet with payload");
}
// To fetch a pallet from a node, the last node must have a pallet
if (!p.Nodes.Last().HasPallet)
{
throw new JobException("Cannot fetch pallet where there is none (Node '" + p.Nodes.Last().Name + "')");
}
}
if (Type == PalletJobType.deliver)
{
// Cannot deliver without payload
if (!f.HasPallet)
{
throw new JobException("Cannot deliver pallet without payload");
}
if (p.Nodes.Last().HasPallet)
{
throw new JobException("Cannot deliver pallet where there is already one (Node '" + p.Nodes.Last().Name + "')");
}
}
if (p.Nodes.Last().Equals(f.FrontNode))
{
throw new JobException("Already at destination '" + f.FrontNode.Name + "'");
}
return(p);
}
public static void Status()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
ConsoleHandler.AddMessage(MessageType.REGULAR, "Status: " + f.Status);
}
