private IRobotCommand GetNextCommand(bool canAcceptMoveCommand) { currentCommand = null; if (commandGenerator == null) { return(null); } if (sendZeroCommand) { currentCommand = commandGenerator.GenerateZeroCommand(); sendZeroCommand = false; } if (sendCancelCommand) { currentCommand = commandGenerator.GenerateCancelCommand(); sendCancelCommand = false; } else if (sendResumeCommand) { currentCommand = commandGenerator.GenerateResumeCommand(); sendResumeCommand = false; } else if (sendPauseCommand) { currentCommand = commandGenerator.GeneratePauseCommand(); sendPauseCommand = false; } else if (sendEnableStepperCommand) { currentCommand = commandGenerator.GenerateStepperEnableCommand(); sendEnableStepperCommand = false; } else if (sendDisableStepperCommand) { currentCommand = commandGenerator.GenerateStepperDisableCommand(); sendDisableStepperCommand = false; } else if (canAcceptMoveCommand && lastPositionKnown) { while (currentCommand == null && commands.Count > 0) { ICommand command = commands.Dequeue(); if (command is MoveTool) { currentCommand = CreateRobotCommand(command as MoveTool, commandGenerator); } } } if (currentCommand == null) { currentCommand = commandGenerator.GenerateStatusCommand(); } return(currentCommand); }
private void NewDataAvailable(SerialPortWrapper.SimpleSerialPacket packet) { lock (thisLock) { if (currentCommand == null) { Console.WriteLine("Error: Received data, but no command was sent!"); foreach (byte b in packet.Data) { Console.Write(b.ToString("x") + ", "); } Console.WriteLine(); } else { currentCommand.ProcessResponse(packet.Data); if (currentCommand.IsDataValid()) { // See if there's any state information in the command used to // update location or other fields... byte locations = MaxLocations; if (currentCommand is StatusCommand) { StatusCommand c = currentCommand as StatusCommand; currentPosition = Vector3.Divide (c.CurrentPosition.ToVector3(), ScaleFactors); locations = c.Locations; if (onPositionUpdate != null) { onPositionUpdate(this, EventArgs.Empty); } } if (currentCommand is MoveCommand) { MoveCommand m = currentCommand as MoveCommand; locations = m.Locations; } currentCommand = GetNextCommand(locations); elapsedCounter = 0; serial.Transmit(currentCommand.GenerateCommand(), 0x21); } else { Console.WriteLine("Error: Did not process data correctly!"); } } } }
internal override bool ProcessResponse(byte data) { if (data == 0xCA) // Only the binary robot will send this character (it's the packet start character) { // Drop anything earlier from the accumulator accumulator.Clear(); Console.WriteLine("Found 0xCA start byte, looks like the packetized binary protocol"); commandGenerator = new PacketizedCommandGenerator(); binaryStatusCommand = commandGenerator.GenerateStatusCommand(); } accumulator.Add(data); if (binaryStatusCommand != null) { // The response will be a binary status command, just forward the data and wait until it's good. return(binaryStatusCommand.ProcessResponse(data)); } else { // Look for an ASCII communicating robot (like GRBL) var s = System.Text.Encoding.ASCII.GetString(accumulator.ToArray()); if (s.EndsWith("\r\n", StringComparison.OrdinalIgnoreCase)) { if (s.StartsWith("Grbl ") && s.Length >= 9) { var version = s.Substring(5, 3); float version_float = 0.0f; if (float.TryParse(version, out version_float) && version_float >= 0.8f) { Console.WriteLine("Compatible Grbl type robot found: " + s.Substring(0, 9)); commandGenerator = new GrblCommandGenerator(); return(true); } } else { // Seems like a GRBL type robot, but the start of the string wasn't right. Maybe some garbage // or an extra \r\n, clear it out and wait for more. accumulator.Clear(); } } } return(false); }
private void NewDataAvailable(byte data) { lock (thisLock) { elapsedCounter = 0; if (currentCommand == null) { Console.WriteLine("Error: Received data, but no command was sent!"); Console.Write(data.ToString("x") + ", "); Console.WriteLine(); } else { if (currentCommand.ProcessResponse(data)) { bool canAcceptMoveCommand = false; if (currentCommand is IRobotCommandWithStatus) { IRobotCommandWithStatus status = currentCommand as IRobotCommandWithStatus; currentPosition = status.CurrentPosition; canAcceptMoveCommand = status.CanAcceptMoveCommand; if (this.lastPositionKnown == false) { lastPosition = currentPosition; lastPositionKnown = true; } if (onRobotStatusChange != null) { onRobotStatusChange(status, EventArgs.Empty); } } else if (currentCommand is RobotDetectionCommand) { RobotDetectionCommand r = currentCommand as RobotDetectionCommand; commandGenerator = r.GetCommandGenerator(); } currentCommand = GetNextCommand(canAcceptMoveCommand); serial.Transmit(currentCommand.GenerateCommand()); } } } }
private IRobotCommand GetNextCommand(byte locations) { currentCommand = null; if (locations < MaxLocations) { // Ok to pass in another movement command // TODO: rework this to use a local buffer... if (onRobotReady != null) { onRobotReady(this, EventArgs.Empty); } } if (currentCommand == null) { currentCommand = new StatusCommand(); } return currentCommand; }
void t_Elapsed(object sender, ElapsedEventArgs e) { t.Stop(); lock (thisLock) { if (serial != null && serial.IsOpen) { elapsedCounter++; if ((elapsedCounter * 50) > (1000)) // More than 1 second to reply { Console.WriteLine("Device Timeout!"); // Send a status command currentCommand = new StatusCommand(); serial.Transmit(currentCommand.GenerateCommand(), 0x21); elapsedCounter = 0; } } } t.Start(); }
/// <summary> /// Run the router from the current position to the given position /// </summary> /// <param name="p">Destination location in inches</param> /// <param name="inches_per_minute">Tool speed in inches per second</param> public override void GoTo(Vector3 p, float inches_per_minute) { lock (thisLock) { Vector3 delta = lastPosition - p; // Z axis has a slower maximum speed. // TODO: implement clamping to a maximum speed for each axis. delta.Z = delta.Z * 10; float inches = delta.Length; Vector3i pointInt = new Vector3i(Vector3.Multiply(p, ScaleFactors)); UInt16 time_milliseconds = (UInt16)(1000 * 60 * inches / inches_per_minute); if (time_milliseconds > 0) { Console.WriteLine("Moving ({0}, {1}, {2}) to ({3}, {4}, {5}) in {6} milliseconds", lastPosition.X, lastPosition.Y, lastPosition.Z, pointInt.x, pointInt.y, pointInt.z, time_milliseconds); lastPosition = p; currentCommand = new MoveCommand(pointInt, time_milliseconds); } else { Console.WriteLine("Ignoring command with time of 0"); } } }
void t_Elapsed(object sender, ElapsedEventArgs e) { t.Stop(); lock (thisLock) { if (elapsedCounter > timeout_ms) { if (currentCommand != null && currentCommand is RobotDetectionCommand) { Console.WriteLine("Unexpected Response from robot detection: " + (currentCommand as RobotDetectionCommand).DumpData()); } // Expected reply not received within 1 second, assume command was lost. Console.WriteLine("Device Timeout!"); } if (serial == null || !serial.IsOpen || elapsedCounter > timeout_ms) { lastPositionKnown = false; commandGenerator = null; currentCommand = null; elapsedCounter = 0; } else { if (currentCommand == null) { currentCommand = new RobotDetectionCommand(); serial.Transmit(currentCommand.GenerateCommand()); elapsedCounter = 0; } else { elapsedCounter += 50; } } } t.Start(); }
internal override bool ProcessResponse(byte data) { if (data == 0xCA) // Only the binary robot will send this character (it's the packet start character) { // Drop anything earlier from the accumulator accumulator.Clear(); Console.WriteLine("Found 0xCA start byte, looks like the packetized binary protocol"); commandGenerator = new PacketizedCommandGenerator(); binaryStatusCommand = commandGenerator.GenerateStatusCommand(); } accumulator.Add(data); if (binaryStatusCommand != null) { // The response will be a binary status command, just forward the data and wait until it's good. return binaryStatusCommand.ProcessResponse(data); } else { // Look for an ASCII communicating robot (like GRBL) var s = System.Text.Encoding.ASCII.GetString(accumulator.ToArray()); if (s.EndsWith("\r\n", StringComparison.OrdinalIgnoreCase)) { if (s.StartsWith("Grbl ") && s.Length >= 9) { var version = s.Substring(5, 3); float version_float = 0.0f; if (float.TryParse(version, out version_float) && version_float >= 0.8f) { Console.WriteLine("Compatible Grbl type robot found: " + s.Substring(0, 9)); commandGenerator = new GrblCommandGenerator(); return true; } } else { // Seems like a GRBL type robot, but the start of the string wasn't right. Maybe some garbage // or an extra \r\n, clear it out and wait for more. accumulator.Clear(); } } } return false; }
private IRobotCommand GetNextCommand(bool canAcceptMoveCommand) { currentCommand = null; if (commandGenerator == null) { return null; } if (sendZeroCommand) { currentCommand = commandGenerator.GenerateZeroCommand(); sendZeroCommand = false; } if (sendCancelCommand) { currentCommand = commandGenerator.GenerateCancelCommand(); sendCancelCommand = false; } else if (sendResumeCommand) { currentCommand = commandGenerator.GenerateResumeCommand(); sendResumeCommand = false; } else if (sendPauseCommand) { currentCommand = commandGenerator.GeneratePauseCommand(); sendPauseCommand = false; } else if (sendEnableStepperCommand) { currentCommand = commandGenerator.GenerateStepperEnableCommand(); sendEnableStepperCommand = false; } else if (sendDisableStepperCommand) { currentCommand = commandGenerator.GenerateStepperDisableCommand(); sendDisableStepperCommand = false; } else if (canAcceptMoveCommand && lastPositionKnown) { while (currentCommand == null && commands.Count > 0) { ICommand command = commands.Dequeue(); if (command is MoveTool) { currentCommand = CreateRobotCommand(command as MoveTool, commandGenerator); } } } if (currentCommand == null) { currentCommand = commandGenerator.GenerateStatusCommand(); } return currentCommand; }