diff --git a/GUI/COMPortForm.Designer.cs b/GUI/COMPortForm.Designer.cs index 330c1a6..a085fa1 100644 --- a/GUI/COMPortForm.Designer.cs +++ b/GUI/COMPortForm.Designer.cs @@ -32,13 +32,13 @@ private void InitializeComponent() this.label1 = new System.Windows.Forms.Label(); this.baudBox = new System.Windows.Forms.TextBox(); this.connect = new System.Windows.Forms.Button(); - this.listBox1 = new System.Windows.Forms.ListBox(); + this.comboBox1 = new System.Windows.Forms.ComboBox(); this.SuspendLayout(); // // label2 // this.label2.AutoSize = true; - this.label2.Location = new System.Drawing.Point(138, 9); + this.label2.Location = new System.Drawing.Point(12, 42); this.label2.Name = "label2"; this.label2.Size = new System.Drawing.Size(58, 13); this.label2.TabIndex = 12; @@ -47,7 +47,7 @@ private void InitializeComponent() // label1 // this.label1.AutoSize = true; - this.label1.Location = new System.Drawing.Point(12, 9); + this.label1.Location = new System.Drawing.Point(12, 15); this.label1.Name = "label1"; this.label1.Size = new System.Drawing.Size(26, 13); this.label1.TabIndex = 11; @@ -55,37 +55,39 @@ private void InitializeComponent() // // baudBox // - this.baudBox.Location = new System.Drawing.Point(138, 25); + this.baudBox.Location = new System.Drawing.Point(76, 39); this.baudBox.Name = "baudBox"; - this.baudBox.Size = new System.Drawing.Size(120, 20); + this.baudBox.Size = new System.Drawing.Size(89, 20); this.baudBox.TabIndex = 10; this.baudBox.Text = "9600"; // // connect // - this.connect.Location = new System.Drawing.Point(138, 51); + this.connect.FlatStyle = System.Windows.Forms.FlatStyle.Flat; + this.connect.Location = new System.Drawing.Point(12, 65); this.connect.Name = "connect"; - this.connect.Size = new System.Drawing.Size(120, 23); + this.connect.Size = new System.Drawing.Size(153, 23); this.connect.TabIndex = 8; this.connect.Text = "Connect"; this.connect.UseVisualStyleBackColor = true; this.connect.Click += new System.EventHandler(this.connect_Click); // - // listBox1 + // comboBox1 // - this.listBox1.FormattingEnabled = true; - this.listBox1.Location = new System.Drawing.Point(12, 25); - this.listBox1.Name = "listBox1"; - this.listBox1.Size = new System.Drawing.Size(120, 69); - this.listBox1.TabIndex = 14; - this.listBox1.SelectedIndexChanged += new System.EventHandler(this.listBox1_SelectedIndexChanged); + this.comboBox1.FlatStyle = System.Windows.Forms.FlatStyle.Flat; + this.comboBox1.FormattingEnabled = true; + this.comboBox1.Location = new System.Drawing.Point(44, 12); + this.comboBox1.Name = "comboBox1"; + this.comboBox1.Size = new System.Drawing.Size(121, 21); + this.comboBox1.TabIndex = 15; + this.comboBox1.SelectedIndexChanged += new System.EventHandler(this.comboBox1_SelectedIndexChanged); // // COMPortForm // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; - this.ClientSize = new System.Drawing.Size(270, 102); - this.Controls.Add(this.listBox1); + this.ClientSize = new System.Drawing.Size(178, 98); + this.Controls.Add(this.comboBox1); this.Controls.Add(this.label2); this.Controls.Add(this.label1); this.Controls.Add(this.baudBox); @@ -103,6 +105,6 @@ private void InitializeComponent() private System.Windows.Forms.Label label1; private System.Windows.Forms.TextBox baudBox; private System.Windows.Forms.Button connect; - private System.Windows.Forms.ListBox listBox1; + private System.Windows.Forms.ComboBox comboBox1; } } \ No newline at end of file diff --git a/GUI/COMPortForm.cs b/GUI/COMPortForm.cs index b289846..38afdcf 100644 --- a/GUI/COMPortForm.cs +++ b/GUI/COMPortForm.cs @@ -39,10 +39,14 @@ public COMPortForm(SerialPortWrapper p) } else { - if (listBox1.SelectedItem == null) + if (comboBox1.SelectedItem == null || comboBox1.SelectedItem == "") { connect.Enabled = false; } + //if (listBox1.SelectedItem == null) + //{ + // connect.Enabled = false; + //} } t = new Timer(); @@ -54,41 +58,47 @@ public COMPortForm(SerialPortWrapper p) void t_Tick(object sender, EventArgs e) { string [] s = port.PortNames; - bool matched = true; - if (s.Length != portNames.Length) + if (s.Length == portNames.Length) { - matched = false; + // No change in items, do nothing. + return; } - if (!matched) - { - object selectedObject = listBox1.SelectedItem; + portNames = s; - portNames = s; - listBox1.Items.Clear(); - listBox1.Items.AddRange(portNames); - - if (selectedObject != null) - { - string str = selectedObject.ToString(); - SelectPortName(str); - } - if (listBox1.SelectedItem == null && !port.IsOpen) - { - connect.Enabled = false; - } - } + string lastSelected = comboBox1.SelectedItem as string; + + comboBox1.Items.Clear(); + comboBox1.Items.AddRange(portNames); + + SelectPortName(lastSelected); + + comboBox1_SelectedIndexChanged(null, EventArgs.Empty); } private void SelectPortName(string name) { - for (int i = 0; i < listBox1.Items.Count; i++) + if (name == null) { - if (listBox1.Items[i].ToString() == name) - { - listBox1.SelectedIndex = i; - } + return; } + var index = comboBox1.Items.IndexOf(name); + if (index >= 0) + { + comboBox1.SelectedIndex = index; + } + else if (name != null && name != "") + { + comboBox1.Items.Insert(0, name); + comboBox1.SelectedIndex = 0; + } + //for (int i = 0; i < listBox1.Items.Count; i++) + //{ + // if (listBox1.Items[i].ToString() == name) + // { + // listBox1.SelectedIndex = i; + // } + //} } private void connect_Click(object sender, EventArgs e) @@ -103,7 +113,7 @@ private void connect_Click(object sender, EventArgs e) try { int baudRate = Convert.ToInt32(this.baudBox.Text); - string portName = listBox1.SelectedItem.ToString(); + string portName = comboBox1.Text; port.Open(portName, baudRate); connect.Text = "Disconnect"; } @@ -123,10 +133,15 @@ private void connect_Click(object sender, EventArgs e) } private void listBox1_SelectedIndexChanged(object sender, EventArgs e) + { + + } + + private void comboBox1_SelectedIndexChanged(object sender, EventArgs e) { if (!port.IsOpen) { - if (listBox1.SelectedItem != null) + if (comboBox1.SelectedItem != null) { this.connect.Enabled = true; } diff --git a/GUI/COMPortForm.resx b/GUI/COMPortForm.resx index d58980a..19dc0dd 100644 --- a/GUI/COMPortForm.resx +++ b/GUI/COMPortForm.resx @@ -112,9 +112,9 @@ 2.0 - System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 \ No newline at end of file diff --git a/GUI/PathCAM.cs b/GUI/PathCAM.cs index baaa991..b028ded 100644 --- a/GUI/PathCAM.cs +++ b/GUI/PathCAM.cs @@ -310,7 +310,7 @@ private void InitializeComponent() this.button2.Name = "button2"; this.button2.Size = new System.Drawing.Size(183, 23); this.button2.TabIndex = 1; - this.button2.Text = "Add Perimeter Paths"; + this.button2.Text = "Add Cutting Paths"; this.button2.UseVisualStyleBackColor = true; this.button2.Click += new System.EventHandler(this.PermiterRoutsClick); // @@ -353,7 +353,7 @@ private void InitializeComponent() this.boundaryCheck.Name = "boundaryCheck"; this.boundaryCheck.Size = new System.Drawing.Size(183, 23); this.boundaryCheck.TabIndex = 2; - this.boundaryCheck.Text = "Boundary Check Paths"; + this.boundaryCheck.Text = "Add Boundary Path"; this.boundaryCheck.UseVisualStyleBackColor = true; this.boundaryCheck.Click += new System.EventHandler(this.boundaryCheckButton_Click); // @@ -411,9 +411,10 @@ private void InitializeComponent() this.robotControl.BackColor = System.Drawing.Color.Transparent; this.robotControl.Location = new System.Drawing.Point(-1, 427); this.robotControl.Name = "robotControl"; - this.robotControl.Size = new System.Drawing.Size(273, 136); + this.robotControl.Size = new System.Drawing.Size(169, 136); this.robotControl.TabIndex = 8; this.robotControl.Visible = false; + this.robotControl.Load += new System.EventHandler(this.robotControl_Load); // // drawing3D // @@ -489,5 +490,10 @@ private void PathCAM_Load(object sender, EventArgs e) robotControl.Location = new Point(0, ClientRectangle.Height - robotControl.Height); showRobotFormCheckbox.Location = new Point(0, ClientRectangle.Height - showRobotFormCheckbox.Height + 1); } + + private void robotControl_Load(object sender, EventArgs e) + { + + } } } diff --git a/GUI/RobotControl.Designer.cs b/GUI/RobotControl.Designer.cs index 561842f..cbc00b5 100644 --- a/GUI/RobotControl.Designer.cs +++ b/GUI/RobotControl.Designer.cs @@ -38,6 +38,7 @@ private void InitializeComponent() this.label1 = new System.Windows.Forms.Label(); this.button1 = new System.Windows.Forms.Button(); this.numericUpDown1 = new System.Windows.Forms.NumericUpDown(); + this.label2 = new System.Windows.Forms.Label(); ((System.ComponentModel.ISupportInitialize)(this.numericUpDown1)).BeginInit(); this.SuspendLayout(); // @@ -47,7 +48,7 @@ private void InitializeComponent() this.steppersEnabledBox.AutoSize = true; this.steppersEnabledBox.Enabled = false; this.steppersEnabledBox.FlatStyle = System.Windows.Forms.FlatStyle.Flat; - this.steppersEnabledBox.Location = new System.Drawing.Point(6, 103); + this.steppersEnabledBox.Location = new System.Drawing.Point(194, 102); this.steppersEnabledBox.Name = "steppersEnabledBox"; this.steppersEnabledBox.Size = new System.Drawing.Size(107, 17); this.steppersEnabledBox.TabIndex = 3; @@ -135,7 +136,7 @@ private void InitializeComponent() // button1 // this.button1.FlatStyle = System.Windows.Forms.FlatStyle.Flat; - this.button1.Location = new System.Drawing.Point(168, 16); + this.button1.Location = new System.Drawing.Point(194, 16); this.button1.Name = "button1"; this.button1.Size = new System.Drawing.Size(75, 23); this.button1.TabIndex = 81; @@ -151,9 +152,9 @@ private void InitializeComponent() 0, 0, 196608}); - this.numericUpDown1.Location = new System.Drawing.Point(168, 76); + this.numericUpDown1.Location = new System.Drawing.Point(65, 102); this.numericUpDown1.Maximum = new decimal(new int[] { - 0, + 10, 0, 0, 0}); @@ -163,13 +164,23 @@ private void InitializeComponent() 0, -2147483648}); this.numericUpDown1.Name = "numericUpDown1"; - this.numericUpDown1.Size = new System.Drawing.Size(75, 20); + this.numericUpDown1.Size = new System.Drawing.Size(97, 20); this.numericUpDown1.TabIndex = 82; this.numericUpDown1.ValueChanged += new System.EventHandler(this.numericUpDown1_ValueChanged); // + // label2 + // + this.label2.AutoSize = true; + this.label2.Location = new System.Drawing.Point(14, 104); + this.label2.Name = "label2"; + this.label2.Size = new System.Drawing.Size(45, 13); + this.label2.TabIndex = 83; + this.label2.Text = "Z Offset"; + // // RobotControl // this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Inherit; + this.Controls.Add(this.label2); this.Controls.Add(this.numericUpDown1); this.Controls.Add(this.button1); this.Controls.Add(this.label1); @@ -181,7 +192,7 @@ private void InitializeComponent() this.Controls.Add(this.cancelButton); this.Controls.Add(this.zbox); this.Name = "RobotControl"; - this.Size = new System.Drawing.Size(248, 124); + this.Size = new System.Drawing.Size(170, 124); ((System.ComponentModel.ISupportInitialize)(this.numericUpDown1)).EndInit(); this.ResumeLayout(false); this.PerformLayout(); @@ -200,6 +211,7 @@ private void InitializeComponent() private System.Windows.Forms.Label label1; private System.Windows.Forms.Button button1; private System.Windows.Forms.NumericUpDown numericUpDown1; + private System.Windows.Forms.Label label2; } } diff --git a/GUI/RobotControl.cs b/GUI/RobotControl.cs index 8436bfe..1694997 100644 --- a/GUI/RobotControl.cs +++ b/GUI/RobotControl.cs @@ -54,7 +54,7 @@ void RobotStatusUpdate(object o, EventArgs e) } else { - StatusCommand status = o as StatusCommand; + IRobotCommandWithStatus status = o as IRobotCommandWithStatus; if (status != null) { this.runButton.Enabled = true; diff --git a/GUI/RobotGUI.cs b/GUI/RobotGUI.cs index 348205a..a1ab120 100644 --- a/GUI/RobotGUI.cs +++ b/GUI/RobotGUI.cs @@ -98,11 +98,11 @@ public PreviousPoint(float time, Vector3 location) Vector3 lastPosition; void RouterPositionUpdate(object o, EventArgs e) { - StatusCommand status = o as StatusCommand; + IRobotCommandWithStatus status = o as IRobotCommandWithStatus; if (status != null) { Vector3 position = status.CurrentPosition; - float time = status.time; + float time = status.Time; float distance = (lastPosition - position).Length; if ((lastPosition - position).Length > 0.0001f) diff --git a/Installer/PathCAM.msi b/Installer/PathCAM.msi index 0ba672f..9b96015 100644 Binary files a/Installer/PathCAM.msi and b/Installer/PathCAM.msi differ diff --git a/Robot/GrblCommandGenerator.cs b/Robot/GrblCommandGenerator.cs new file mode 100644 index 0000000..e1ef397 --- /dev/null +++ b/Robot/GrblCommandGenerator.cs @@ -0,0 +1,260 @@ +using OpenTK; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace Robot +{ + class GrblCommandGenerator : ICommandGenerator + { + private StringBuilder dataBuffer; + + public GrblCommandGenerator() + { + dataBuffer = new StringBuilder(); + } + + // All data from the serial port comes through this function via the GrblStatusCommand (or inherited commands) + private string ProcessGrblByte(byte b) + { + dataBuffer.Append((char)b); + + var response = dataBuffer.ToString(); + if (response.EndsWith(">\r\n")) + { + // Found the last character in an unbuffered command + var start = response.IndexOf('<'); + var unbuffered_command = response.Substring(start, response.Length - (start + 2)); + dataBuffer.Remove(start, dataBuffer.Length - start); + return unbuffered_command; + } + else if (response.EndsWith("\r\n")) + { + var buffered_command = response.Substring(0, response.Length - 2); + dataBuffer = new StringBuilder(); + return buffered_command; + } + + return null; + } + + private class GrblStatusCommand : IRobotCommandWithStatus + { + private Vector3 location = new Vector3(0, 0, 0); + private GrblCommandGenerator parent; + private bool canAcceptMoveCommand = false; + private bool paused = false; + private bool pausing = false; + + public GrblStatusCommand(GrblCommandGenerator parent) + { + this.parent = parent; + } + + internal override byte[] GenerateCommand() + { + //Console.WriteLine("Sending ?"); + return new byte[] { (byte)'?' }; + } + + internal override bool ProcessResponse(byte data) + { + var result = parent.ProcessGrblByte(data); + if (result != null) + { + Console.WriteLine("Received GRBL Data: " + result); + if (result.Equals("ok", StringComparison.OrdinalIgnoreCase)) + { + canAcceptMoveCommand = true; + } + else if (result.StartsWith("<") && result.EndsWith(">")) + { + // Status response will look like this: + // + // TODO: more robust parsing for the GRBL status string + string inside = result.Substring(1, result.Length - 2); + bool mpos_found = false; + List position = new List(); + foreach (var s in inside.Split(new char[] { ',', ':' })) + { + if (mpos_found && position.Count < 3) + { + position.Add(float.Parse(s)); + } + else if (s.Equals("mpos", StringComparison.OrdinalIgnoreCase)) + { + mpos_found = true; + } + else if (s.Equals("idle", StringComparison.OrdinalIgnoreCase)) + { + canAcceptMoveCommand = true; + } + else if (s.Equals("queue", StringComparison.OrdinalIgnoreCase)) + { + paused = true; + } + else if (s.Equals("hold", StringComparison.OrdinalIgnoreCase)) + { + pausing = true; + } + } + if (position.Count != 3) + { + return false; + } + location.X = position[0]; + location.Y = position[1]; + location.Z = position[2]; + location = location / 25.4f; + return true; + } + else + { + Console.WriteLine("Command Not Understood: " + result); + } + } + return false; + } + + public override bool Paused + { + get { return paused; } + } + + public override bool Pausing + { + get { return pausing; } + } + + public override bool SteppersEnabled + { + get { return true; } + } + + public override Vector3 CurrentPosition + { + get { return location; } + } + + public override float Time + { + get { return 1.0f; } + } + + public override bool CanAcceptMoveCommand + { + get { return canAcceptMoveCommand; } + } + } + + private class GrblMoveCommand : GrblStatusCommand + { + private float target_mm_per_minute; + private Vector3 toLocation = new Vector3(0, 0, 0); + + public GrblMoveCommand(GrblCommandGenerator parent, Vector3 location, float inches_per_second) + : base(parent) + { + toLocation = location; + target_mm_per_minute = inches_per_second * 25.4f * 60.0f; + } + + internal override byte[] GenerateCommand() + { + var target_mm = toLocation * 25.4f; + String s = String.Format("F{0:F3}\r\nG1 X{1:F4} Y{2:F4} Z{3:F4}\r\n?", target_mm_per_minute, target_mm.X, target_mm.Y, target_mm.Z); + Console.WriteLine("Sending: " + s); + return System.Text.Encoding.ASCII.GetBytes(s); + } + } + + private class GrblPauseCommand : GrblStatusCommand + { + public GrblPauseCommand(GrblCommandGenerator parent) : base(parent) { } + + internal override byte[] GenerateCommand() + { + return new byte [] {(byte)'!', (byte)'?'}; + } + } + + private class GrblResumeCommand : GrblStatusCommand + { + public GrblResumeCommand(GrblCommandGenerator parent) : base(parent) { } + + internal override byte[] GenerateCommand() + { + return new byte[] { (byte)'~', (byte)'?' }; + } + } + + private class GrblCancelCommand : GrblStatusCommand + { + public GrblCancelCommand(GrblCommandGenerator parent) : base(parent) { } + + internal override byte[] GenerateCommand() + { + return new byte[] { 0x18, (byte)'?' }; + } + } + + private class GrblHomeCommand : GrblStatusCommand + { + public GrblHomeCommand(GrblCommandGenerator parent) : base(parent) { } + + internal override byte[] GenerateCommand() + { + return System.Text.Encoding.ASCII.GetBytes("$H\r\n?"); + } + } + + public override IRobotCommand GenerateMoveCommand(OpenTK.Vector3 location, float inches_per_second) + { + return new GrblMoveCommand(this, location, inches_per_second); + } + + public override IRobotCommand GenerateStatusCommand() + { + return new GrblStatusCommand(this); + } + + public override IRobotCommand GenerateResetCommand() + { + //throw new NotImplementedException(); + return new GrblStatusCommand(this); + } + + public override IRobotCommand GenerateZeroCommand() + { + return new GrblHomeCommand(this); + } + + public override IRobotCommand GeneratePauseCommand() + { + return new GrblPauseCommand(this); + } + + public override IRobotCommand GenerateResumeCommand() + { + return new GrblResumeCommand(this); + } + + public override IRobotCommand GenerateCancelCommand() + { + return new GrblCancelCommand(this); + } + + public override IRobotCommand GenerateStepperEnableCommand() + { + //throw new NotImplementedException(); + return new GrblStatusCommand(this); + } + + public override IRobotCommand GenerateStepperDisableCommand() + { + //throw new NotImplementedException(); + return new GrblStatusCommand(this); + } + } +} diff --git a/Robot/ICommandGenerator.cs b/Robot/ICommandGenerator.cs new file mode 100644 index 0000000..5524db5 --- /dev/null +++ b/Robot/ICommandGenerator.cs @@ -0,0 +1,21 @@ +using OpenTK; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace Robot +{ + abstract class ICommandGenerator + { + public abstract IRobotCommand GenerateMoveCommand(Vector3 location, float inches_per_second); + public abstract IRobotCommand GenerateStatusCommand(); + public abstract IRobotCommand GenerateResetCommand(); + public abstract IRobotCommand GenerateZeroCommand(); + public abstract IRobotCommand GeneratePauseCommand(); + public abstract IRobotCommand GenerateResumeCommand(); + public abstract IRobotCommand GenerateCancelCommand(); + public abstract IRobotCommand GenerateStepperEnableCommand(); + public abstract IRobotCommand GenerateStepperDisableCommand(); + } +} diff --git a/Robot/IRobotCommand.cs b/Robot/IRobotCommand.cs index c6ef2e1..e120f9b 100644 --- a/Robot/IRobotCommand.cs +++ b/Robot/IRobotCommand.cs @@ -20,13 +20,25 @@ using System.Collections.Generic; using System.Linq; using System.Text; +using OpenTK; namespace Robot { public abstract class IRobotCommand { internal abstract byte[] GenerateCommand(); - internal abstract void ProcessResponse(byte[] data); - internal abstract bool IsDataValid(); + internal abstract bool ProcessResponse(byte data); + //internal abstract bool IsDataValid(); + } + + public abstract class IRobotCommandWithStatus : IRobotCommand + { + // Add in some properties for the current robot status + public abstract bool Paused { get; } + public abstract bool Pausing { get; } + public abstract bool SteppersEnabled { get; } + public abstract Vector3 CurrentPosition { get; } + public abstract float Time { get; } + public abstract bool CanAcceptMoveCommand { get; } } } diff --git a/Robot/MoveCommand.cs b/Robot/MoveCommand.cs deleted file mode 100644 index 5d42fb9..0000000 --- a/Robot/MoveCommand.cs +++ /dev/null @@ -1,54 +0,0 @@ -/* - * PathCAM - Toolpath generation software for CNC manufacturing machines - * Copyright (C) 2013 Benjamin R. Porter https://github.com/xenovacivus - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see [http://www.gnu.org/licenses/]. - */ - -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using OpenTK; - -namespace Robot -{ - internal class MoveCommand : StatusCommand - { - protected override byte CommandCode - { - get { return 0x32; } - } - - private UInt16 thousandths_per_second; - private Vector3 toLocation = new Vector3(0, 0, 0); - - public MoveCommand(Vector3 location, float inches_per_second) : base() - { - toLocation = location; - this.thousandths_per_second = (UInt16)(inches_per_second * 1000); - } - - internal override byte[] GenerateCommand() - { - List command = new List(); - command.Add(CommandCode); - command.AddRange(DataConverter.BytesFromShort((short)thousandths_per_second)); - command.AddRange(DataConverter.BytesFromFloat(toLocation.X)); - command.AddRange(DataConverter.BytesFromFloat(toLocation.Y)); - command.AddRange(DataConverter.BytesFromFloat(toLocation.Z)); - return command.ToArray(); - } - } -} diff --git a/Robot/PacketizedCommandGenerator.cs b/Robot/PacketizedCommandGenerator.cs new file mode 100644 index 0000000..8ace955 --- /dev/null +++ b/Robot/PacketizedCommandGenerator.cs @@ -0,0 +1,430 @@ +using OpenTK; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace Robot +{ + partial class PacketizedCommandGenerator : ICommandGenerator + { + #region Serial Packetizer and Depacketize State Machines + + private class SerialPacket + { + /** Serial Packet Definition: + // + // START_BYTE start byte + // address + // length + // data(n bytes) + // Checksum ~(address + length + data) + + // Escape Character and Byte Stuffing: + // Any control character is replaced + // with an escape character followed + // by it's "escaped" value. All data + // in the transmission except for the + // start byte can be escaped. This + // means the transmission may take up + // to twice as long as expected from + // just transmitting escape character + **/ + + + /* Control Characters and thier Escaped Equivelants */ + const byte START_BYTE = 0xCA; + const byte ESCAPE_CHAR = (byte)'\\'; + const byte null_BYTE = 0; + const byte MAX_BYTE = 255; + const byte START_BYTE_ESCAPED = 2; + const byte ESCAPE_CHAR_ESCAPED = 3; + const byte null_BYTE_ESCAPED = 4; + const byte MAX_BYTE_ESCAPED = 5; + + + private enum ReceiveState + { + AwaitingStartByte, + AwaitingAddress, + AwaitingLength, + AwaitingData, + AwaitingChecksum, + } + + ReceiveState receive_state; + bool receive_next_char_is_escaped; + byte receive_address; + byte receive_checksum; + byte receive_length; + List receive_data; + public SerialPacket() + { + receive_state = ReceiveState.AwaitingStartByte; + receive_next_char_is_escaped = false; + receive_address = 0; + receive_checksum = 0; + receive_length = 0; + receive_data = new List(); + } + + /// + /// Feed a byte to the serial depacketizing state machine + /// + /// + /// non-null byte [] when a packet is received completely + public byte[] ProcessByte(byte data) + { + // Check for invalid data conditions, ignore any bad bytes + if (data == null_BYTE || data == MAX_BYTE) + { + Console.WriteLine("Serial Error: Got bad byte from serial stream"); + return null; + } + + if (receive_state == ReceiveState.AwaitingStartByte && data != START_BYTE) + { + Console.WriteLine("Serial Error: Got an unexpected byte while waiting for start byte"); + return null; + } + + if (receive_state != ReceiveState.AwaitingStartByte && data == START_BYTE) + { + Console.WriteLine("Serial Error: start byte found while already processing a packet"); + return null; + } + + // Check if the next byte is escaped + if (data == ESCAPE_CHAR) + { + receive_next_char_is_escaped = true; + return null; + } + if (receive_next_char_is_escaped) + { + if (data == ESCAPE_CHAR_ESCAPED) { + data = ESCAPE_CHAR; + } else if (data == START_BYTE_ESCAPED) { + data = START_BYTE; + } else if (data == null_BYTE_ESCAPED) { + data = null_BYTE; + } else if (data == MAX_BYTE_ESCAPED) { + data = MAX_BYTE; + } + receive_next_char_is_escaped = false; + } + + switch (receive_state) + { + case ReceiveState.AwaitingStartByte: + receive_next_char_is_escaped = false; + receive_state = ReceiveState.AwaitingAddress; + break; + + case ReceiveState.AwaitingAddress: + receive_address = data; + receive_checksum = data; + receive_state = ReceiveState.AwaitingLength; + break; + + case ReceiveState.AwaitingLength: + receive_length = data; + receive_checksum += data; + receive_data.Clear(); + receive_state = ReceiveState.AwaitingData; + break; + + case ReceiveState.AwaitingData: + receive_checksum += data; + receive_data.Add(data); + if (--receive_length == 0) + { + receive_state = ReceiveState.AwaitingChecksum; + } + break; + + case ReceiveState.AwaitingChecksum: + receive_state = ReceiveState.AwaitingStartByte; + receive_checksum = (byte)~receive_checksum; + if (data != receive_checksum) + { + Console.WriteLine("Serial Error: Checksum Mismatch"); + return null; + } + return receive_data.ToArray(); + } + return null; + } + + + + public static byte[] Packetize(byte[] data, byte address) + { + byte length = (byte)data.Length; + List packet = new List(); + + packet.Add(START_BYTE); + packet.AddRange(Escape(address)); + packet.AddRange(Escape(length)); + + byte checksum = (byte)(length + address); + foreach (byte b in data) + { + checksum += b; + packet.AddRange(Escape(b)); + } + packet.AddRange(Escape((byte)~checksum)); + + return packet.ToArray(); + } + + private static byte[] Escape(byte data) + { + switch (data) + { + case START_BYTE: + return new byte[] { ESCAPE_CHAR, START_BYTE_ESCAPED }; + case ESCAPE_CHAR: + return new byte[] { ESCAPE_CHAR, ESCAPE_CHAR_ESCAPED }; + case null_BYTE: + return new byte[] { ESCAPE_CHAR, null_BYTE_ESCAPED }; + case MAX_BYTE: + return new byte[] { ESCAPE_CHAR, MAX_BYTE_ESCAPED }; + default: + return new byte[] { data }; + } + } + } + + #endregion + + + #region Command Definitions + + private class StatusCommand : IRobotCommandWithStatus + { + private SerialPacket depacketizer = new SerialPacket(); + private Vector3 currentPosition = new Vector3(0, 0, 0); + private bool data_valid = false; + //private bool is_moving = false; + private int locations; + + // TODO: find a better way to handle the time + private float time; + + public override float Time { get { return time; } } + + + public StatusCommand() + { + } + + protected virtual byte CommandCode + { + get { return 0x77; } + } + + public override bool CanAcceptMoveCommand + { + get { return locations > 0; } + } + + public override Vector3 CurrentPosition + { + get { return currentPosition; } + } + + //internal override bool IsDataValid() + //{ + // return data_valid; + //} + + //internal bool IsMoving() + //{ + // return is_moving; + //} + + internal override byte[] GenerateCommand() + { + return SerialPacket.Packetize(new byte[] { CommandCode }, 0x21); + } + + bool paused = false; + bool pausing = false; + bool steppers_enabled = false; + public override bool Paused { get { return paused; } } + public override bool Pausing { get { return pausing; } } + public override bool SteppersEnabled { get { return steppers_enabled; } } + + List data = new List(); + internal override bool ProcessResponse(byte b) + { + byte[] data = depacketizer.ProcessByte(b); + if (data == null) + { + return false; + } + + // TODO: need to depacketize the data... + //data.Add(b); + //if (data.Count <= 18) + //{ + // return false; + //} + data_valid = (data[0] == CommandCode); + + List data_list = new List(data); + + time = ((float)DataConverter.IntFromBytes(data_list.GetRange(15, 4))) / 10.0f; + currentPosition = new Vector3( + DataConverter.FloatFromBytes(data_list.GetRange(1, 4)), + DataConverter.FloatFromBytes(data_list.GetRange(5, 4)), + DataConverter.FloatFromBytes(data_list.GetRange(9, 4))); + + byte status_bits = data[13]; + paused = (status_bits & 0x01) > 0; + pausing = (status_bits & 0x02) > 0; + steppers_enabled = (status_bits & 0x04) > 0; + + //is_moving = true;// x_moving | y_moving | z_moving; + + locations = (int)(data[14]); + return true; + //Console.WriteLine("{0}, {1}, {2}, {3}, l = {4}, paused = {5}, pausing = {6}, resuming = {7}", time, currentPosition.X, currentPosition.Y, currentPosition.Z, locations, paused, pausing, resuming); + } + + } + + private class MoveCommand : StatusCommand + { + protected override byte CommandCode + { + get { return 0x32; } + } + + private UInt16 thousandths_per_second; + private Vector3 toLocation = new Vector3(0, 0, 0); + + public MoveCommand(Vector3 location, float inches_per_second) + : base() + { + toLocation = location; + this.thousandths_per_second = (UInt16)(inches_per_second * 1000); + } + + internal override byte[] GenerateCommand() + { + List command = new List(); + command.Add(CommandCode); + command.AddRange(DataConverter.BytesFromShort((short)thousandths_per_second)); + command.AddRange(DataConverter.BytesFromFloat(toLocation.X)); + command.AddRange(DataConverter.BytesFromFloat(toLocation.Y)); + command.AddRange(DataConverter.BytesFromFloat(toLocation.Z)); + return SerialPacket.Packetize(command.ToArray(), 0x21); + } + } + + private abstract class SingleByteStatusCommand : StatusCommand + { + internal override byte[] GenerateCommand() + { + return SerialPacket.Packetize(new byte[] { CommandCode }, 0x21); + } + } + + private class StepperDisableCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x14; } } + public StepperDisableCommand() : base() { } + } + + private class StepperEnableCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x15; } } + public StepperEnableCommand() : base() { } + } + + private class CancelCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x13; } } + public CancelCommand() : base() { } + } + + private class PauseCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x11; } } + public PauseCommand() : base() { } + } + + private class ResumeCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x12; } } + public ResumeCommand() : base() { } + } + + private class ResetCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x88; } } + public ResetCommand() : base() { } + } + + private class ZeroCommand : SingleByteStatusCommand + { + protected override byte CommandCode { get { return 0x90; } } + public ZeroCommand() : base() { } + } + + #endregion + + + #region ICommandGenerator Implementation + + public override IRobotCommand GenerateMoveCommand(Vector3 location, float inches_per_second) + { + return new MoveCommand(location, inches_per_second); + } + + public override IRobotCommand GenerateStatusCommand() + { + return new StatusCommand(); + } + + public override IRobotCommand GenerateResetCommand() + { + return new ResetCommand(); + } + + public override IRobotCommand GenerateZeroCommand() + { + return new ZeroCommand(); + } + + public override IRobotCommand GeneratePauseCommand() + { + return new PauseCommand(); + } + + public override IRobotCommand GenerateResumeCommand() + { + return new ResumeCommand(); + } + + public override IRobotCommand GenerateCancelCommand() + { + return new CancelCommand(); + } + + public override IRobotCommand GenerateStepperEnableCommand() + { + return new StepperEnableCommand(); + } + + public override IRobotCommand GenerateStepperDisableCommand() + { + return new StepperDisableCommand(); + } + + #endregion + + } +} diff --git a/Robot/Robot.cs b/Robot/Robot.cs index 3db8d60..f8b01c2 100644 --- a/Robot/Robot.cs +++ b/Robot/Robot.cs @@ -132,39 +132,131 @@ public float MaxRapidSpeed } - + ICommandGenerator commandGenerator; public Robot(SerialPortWrapper serial) { + commandGenerator = null; this.serial = serial; serial.newDataAvailable += new SerialPortWrapper.newDataAvailableDelegate(NewDataAvailable); - serial.receiveDataError += new SerialPortWrapper.receiveDataErrorDelegate(ReceiveDataError); t = new Timer(); t.Interval = 50; t.Start(); t.Elapsed += new ElapsedEventHandler(t_Elapsed); } + /// + /// Special command that can detect the type of robot connected and + /// create the proper ICommandGenerator. + /// + private class RobotDetectionCommand : IRobotCommand + { + List accumulator = new List(); + IRobotCommand binaryStatusCommand; + //private StringBuilder accumulator = new StringBuilder(); + private ICommandGenerator commandGenerator = null; + internal override byte[] GenerateCommand() + { + // This command works both as a status command for the binary format + // And as a reset command for the Grbl commands. + return new byte[] { 0xCA, 0x21, 0x02, 0x77, 0x4D, 0x18 }; + } + + 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; + } + + internal string DumpData() + { + // Create a string containing the bytes received in both hex and characters + var s = System.Text.Encoding.ASCII.GetString(accumulator.ToArray()); + var retString = "{ " + string.Join(" ", accumulator.Select(b => string.Format("{0:X2}", b)).ToArray()) + " } " + s.TrimEnd(new char [] {'\r', '\n'}); + return retString; + } + + internal ICommandGenerator GetCommandGenerator() + { + return commandGenerator; + } + } + + const int timeout_ms = 1000; + void t_Elapsed(object sender, ElapsedEventArgs e) { t.Stop(); lock (thisLock) { - if (serial != null && serial.IsOpen) + if (elapsedCounter > timeout_ms) { - elapsedCounter++; - if ((elapsedCounter * 50) > (1000)) // More than 1 second to reply + if (currentCommand != null && currentCommand is RobotDetectionCommand) { - Console.WriteLine("Device Timeout!"); - - // Assume disconnected, won't give another move command until the position is known - lastPositionKnown = false; - - // Send a status command - currentCommand = new StatusCommand(); - - serial.Transmit(currentCommand.GenerateCommand(), 0x21); + 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(); @@ -176,7 +268,7 @@ private void ReceiveDataError(byte err) } Object thisLock = new Object(); - private void NewDataAvailable(SerialPortWrapper.SimpleSerialPacket packet) + private void NewDataAvailable(byte data) { lock (thisLock) { @@ -184,23 +276,19 @@ private void NewDataAvailable(SerialPortWrapper.SimpleSerialPacket packet) 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.Write(data.ToString("x") + ", "); Console.WriteLine(); } else { - currentCommand.ProcessResponse(packet.Data); - if (currentCommand.IsDataValid()) + if (currentCommand.ProcessResponse(data)) { - int locations = 0; - if (currentCommand is StatusCommand) + bool canAcceptMoveCommand = false; + if (currentCommand is IRobotCommandWithStatus) { - StatusCommand c = currentCommand as StatusCommand; - currentPosition = c.CurrentPosition; - locations = c.Locations; + IRobotCommandWithStatus status = currentCommand as IRobotCommandWithStatus; + currentPosition = status.CurrentPosition; + canAcceptMoveCommand = status.CanAcceptMoveCommand; if (this.lastPositionKnown == false) { lastPosition = currentPosition; @@ -208,72 +296,74 @@ private void NewDataAvailable(SerialPortWrapper.SimpleSerialPacket packet) } if (onRobotStatusChange != null) { - onRobotStatusChange(c, EventArgs.Empty); + onRobotStatusChange(status, EventArgs.Empty); } } - if (currentCommand is MoveCommand) + else if (currentCommand is RobotDetectionCommand) { + RobotDetectionCommand r = currentCommand as RobotDetectionCommand; + commandGenerator = r.GetCommandGenerator(); } - currentCommand = GetNextCommand(locations); - serial.Transmit(currentCommand.GenerateCommand(), 0x21); - } - else - { - Console.WriteLine("Error: Did not process data correctly!"); + currentCommand = GetNextCommand(canAcceptMoveCommand); + serial.Transmit(currentCommand.GenerateCommand()); } } } } - private IRobotCommand GetNextCommand(int locations) + private IRobotCommand GetNextCommand(bool canAcceptMoveCommand) { currentCommand = null; + if (commandGenerator == null) + { + return null; + } if (sendZeroCommand) { - currentCommand = new ZeroCommand(); + currentCommand = commandGenerator.GenerateZeroCommand(); sendZeroCommand = false; } if (sendCancelCommand) { - currentCommand = new CancelCommand(); + currentCommand = commandGenerator.GenerateCancelCommand(); sendCancelCommand = false; } else if (sendResumeCommand) { - currentCommand = new ResumeCommand(); + currentCommand = commandGenerator.GenerateResumeCommand(); sendResumeCommand = false; } else if (sendPauseCommand) { - currentCommand = new PauseCommand(); + currentCommand = commandGenerator.GeneratePauseCommand(); sendPauseCommand = false; } else if (sendEnableStepperCommand) { - currentCommand = new StepperEnableCommand(); + currentCommand = commandGenerator.GenerateStepperEnableCommand(); sendEnableStepperCommand = false; } else if (sendDisableStepperCommand) { - currentCommand = new StepperDisableCommand(); + currentCommand = commandGenerator.GenerateStepperDisableCommand(); sendDisableStepperCommand = false; } - else if (locations > 0 && lastPositionKnown) + else if (canAcceptMoveCommand && lastPositionKnown) { while (currentCommand == null && commands.Count > 0) { ICommand command = commands.Dequeue(); if (command is MoveTool) { - currentCommand = CreateRobotCommand(command as MoveTool); + currentCommand = CreateRobotCommand(command as MoveTool, commandGenerator); } } } if (currentCommand == null) { - currentCommand = new StatusCommand(); + currentCommand = commandGenerator.GenerateStatusCommand(); } return currentCommand; @@ -285,7 +375,7 @@ private IRobotCommand GetNextCommand(int locations) /// /// /// - private IRobotCommand CreateRobotCommand(MoveTool m) + private IRobotCommand CreateRobotCommand(MoveTool m, ICommandGenerator c) { var p = m.Target; p.Z += z_offset; @@ -301,8 +391,7 @@ private IRobotCommand CreateRobotCommand(MoveTool m) { inches_per_minute = Math.Min(MaxZSpeed, inches_per_minute); } - - return new MoveCommand(p, inches_per_minute / 60.0f); + return c.GenerateMoveCommand(p, inches_per_minute / 60.0f); } else { diff --git a/Robot/Robot.csproj b/Robot/Robot.csproj index 9d18609..ce32d3a 100644 --- a/Robot/Robot.csproj +++ b/Robot/Robot.csproj @@ -45,12 +45,12 @@ + + - + - - @@ -62,6 +62,7 @@ Serial +