Skip to content

Commit

Permalink
Added changes to support homing to zero
Browse files Browse the repository at this point in the history 
Zero button: goes from the current position to the limit switches, the
resumes earlier activity
Z Offset: necessary to drop the tool down to the work level
  • Loading branch information
@xenovacivus
xenovacivus committed Mar 31, 2014
1 parent ce8b8a5 commit 17e950b
Show file tree
Hide file tree
Showing 7 changed files with 229 additions and 88 deletions.
2 changes: 1 addition & 1 deletion GUI/PathCAM.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -411,7 +411,7 @@ 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(169, 136);
this.robotControl.Size = new System.Drawing.Size(273, 136);
this.robotControl.TabIndex = 8;
this.robotControl.Visible = false;
//
Expand Down
45 changes: 44 additions & 1 deletion GUI/RobotControl.Designer.cs
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

15 changes: 14 additions & 1 deletion GUI/RobotControl.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,9 @@ void IOpenGLDrawable.Draw()
GL.Color3(Color.Silver);
Polyhedra.DrawCone(position + new Vector3(0, 0, router.ToolDiameter), position, router.ToolDiameter / 2.0f);


Vector3 physicalPosition = robot.GetPhysicalPosition();
GL.Color3(Color.Black);
Polyhedra.DrawCone(physicalPosition + new Vector3(0, 0, router.ToolDiameter), physicalPosition, router.ToolDiameter / 2.0f);


//// Draw the past positions & velocity graph
Expand Down Expand Up @@ -221,6 +223,17 @@ private void button4_Click(object sender, EventArgs e)
}
}

private void button1_Click(object sender, EventArgs e)
{
robot.Zero();
}

private void numericUpDown1_ValueChanged(object sender, EventArgs e)
{
robot.z_offset = (float)numericUpDown1.Value;
this.zGo_Click(null, EventArgs.Empty);
}

//private List<PreviousPoint> previousPoints = new List<PreviousPoint>();
//public class PreviousPoint
//{
Expand Down
169 changes: 85 additions & 84 deletions GUI/RobotGUI.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ public class RobotGUI : Robot.Robot, IOpenGLDrawable
public RobotGUI(SerialPortWrapper serial, Router.Router router) : base (serial)
{
this.router = router;
base.onRobotStatusChange += new EventHandler (RouterPositionUpdate);
}

void IOpenGLDrawable.Draw()
Expand All @@ -30,95 +31,95 @@ void IOpenGLDrawable.Draw()



//// Draw the past positions & velocity graph
//float lastTime = 0;
//Vector3 lastPos = new Vector3(0, 0, 0);
//float lastVel = 0;
//bool lastIsGood = false;
//GL.Disable(EnableCap.Lighting);
//lock (previousPoints)
//{
// Vector3 lastpoint = new Vector3(0, 0, 0);
// for (int i = 0; i < previousPoints.Count(); i++)
// {
// PreviousPoint point = previousPoints[i];
// float age_delta = point.createTime - lastTime;
// float time = age_delta / 1000.0f; // Age is microseconds, time is seconds
// float pos_delta = (point.location - lastPos).Length;
// float vel = pos_delta / time; // Inches per second
// Vector3 atpoint = new Vector3(point.location.X * 1000, point.location.Y * 1000, point.location.Z * 1000);
// if (lastIsGood)
// {
// GL.LineWidth(1);
// GL.Begin(PrimitiveType.Lines);
// GL.Color3(Color.LightGray);
// for (int j = 0; j < 5; j++)
// {
// GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10));
// GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10 + 10));
//
// GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10));
// GL.Vertex3(atpoint + new Vector3(0, 0, j * 10));
// }
// GL.End();
// GL.LineWidth(2);
// GL.Begin(PrimitiveType.Lines);
// GL.Color3(Color.Orange);
// GL.Vertex3(lastpoint + new Vector3(0, 0, lastVel * lastVel * 200));
// GL.Vertex3(atpoint + new Vector3(0, 0, vel * vel * 200));
// GL.End();
// }
// lastVel = vel;
// lastpoint = atpoint;
//
// lastPos = point.location;
// lastTime = point.createTime;
// lastIsGood = true;
// }
//}
//GL.Enable(EnableCap.Lighting);
//GL.LineWidth(1);
// Draw the past positions & velocity graph
float lastTime = 0;
Vector3 lastPos = new Vector3(0, 0, 0);
float lastVel = 0;
bool lastIsGood = false;
GL.Disable(EnableCap.Lighting);
lock (previousPoints)
{
Vector3 lastpoint = new Vector3(0, 0, 0);
for (int i = 0; i < previousPoints.Count(); i++)
{
PreviousPoint point = previousPoints[i];
float age_delta = point.createTime - lastTime;
float time = age_delta / 1000.0f; // Age is microseconds, time is seconds
float pos_delta = (point.location - lastPos).Length;
float vel = pos_delta / time; // Inches per second
Vector3 atpoint = new Vector3(point.location.X * 1000, point.location.Y * 1000, point.location.Z * 1000);
if (lastIsGood)
{
GL.LineWidth(1);
GL.Begin(PrimitiveType.Lines);
GL.Color3(Color.LightGray);
for (int j = 0; j < 5; j++)
{
GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10));
GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10 + 10));

GL.Vertex3(lastpoint + new Vector3(0, 0, j * 10));
GL.Vertex3(atpoint + new Vector3(0, 0, j * 10));
}
GL.End();
GL.LineWidth(2);
GL.Begin(PrimitiveType.Lines);
GL.Color3(Color.Orange);
GL.Vertex3(lastpoint + new Vector3(0, 0, lastVel * lastVel * 200));
GL.Vertex3(atpoint + new Vector3(0, 0, vel * vel * 200));
GL.End();
}
lastVel = vel;
lastpoint = atpoint;

lastPos = point.location;
lastTime = point.createTime;
lastIsGood = true;
}
}
GL.Enable(EnableCap.Lighting);
GL.LineWidth(1);

}


//private List<PreviousPoint> previousPoints = new List<PreviousPoint>();
//public class PreviousPoint
//{
// public PreviousPoint(float time, Vector3 location)
// {
// this.createTime = time;
// this.location = location;
// }
// public float createTime;
// public Vector3 location;
//}

private List<PreviousPoint> previousPoints = new List<PreviousPoint>();
public class PreviousPoint
{
public PreviousPoint(float time, Vector3 location)
{
this.createTime = time;
this.location = location;
}
public float createTime;
public Vector3 location;
}

//void RouterPositionUpdate(object o, EventArgs e)
//{
// StatusCommand status = o as StatusCommand;
// if (status != null)
// {
// Vector3 position = status.CurrentPosition;
// float time = status.time;
// float distance = (lastPosition - position).Length;
//
// if ((lastPosition - position).Length > 0.0001f)
// {
// lock (previousPoints)
// {
// //Console.WriteLine("{0},{1}", time, distance);
// while (previousPoints.Count > 1000)
// {
// previousPoints.RemoveAt(0);
// }
// previousPoints.Add(new PreviousPoint(time, position));
// lastPosition = position;
// }
// }
// }
//}
Vector3 lastPosition;
void RouterPositionUpdate(object o, EventArgs e)
{
StatusCommand status = o as StatusCommand;
if (status != null)
{
Vector3 position = status.CurrentPosition;
float time = status.time;
float distance = (lastPosition - position).Length;

if ((lastPosition - position).Length > 0.0001f)
{
lock (previousPoints)
{
//Console.WriteLine("{0},{1}", time, distance);
while (previousPoints.Count > 1000)
{
previousPoints.RemoveAt(0);
}
previousPoints.Add(new PreviousPoint(time, position));
lastPosition = position;
}
}
}
}

}
}
54 changes: 54 additions & 0 deletions Geometry/AnalyzedTriangleMesh.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,60 @@ public AnalyzedTriangleMesh(TriangleMesh mesh)
}
}

/// <summary>
/// Split a mesh into meshes of connected triangles
/// </summary>
/// <returns></returns>
public List<TriangleMesh> SplitDisconnected()
{
List<TriangleMesh> meshes = new List<TriangleMesh>();

List<TriangleIndices> tempTriangles = new List<TriangleIndices>();

tempTriangles = triangles.ToList();

while(tempTriangles.Count > 0)
{
var mesh = new TriangleMesh();
var first = tempTriangles[0];
var connected = GetConnected(ref tempTriangles, first);
foreach (var indices in connected)
{
mesh.AddTriangle(new Triangle(vertices[indices.a], vertices[indices.b], vertices[indices.c]));
}
meshes.Add(mesh);
}

return meshes;
}

public List<TriangleIndices> GetConnected(ref List<TriangleIndices> tempTriangles, TriangleIndices first)
{
List<TriangleIndices> connected = new List<TriangleIndices>();
connected.Add(first);
tempTriangles.Remove(first);

foreach (var edge in first.edges)
{
foreach (var tri in edge.triangles)
{
if (tempTriangles.Contains(tri))
{
connected.AddRange(GetConnected(ref tempTriangles, tri));
}
}
}

return connected;
}

//public List<Triangles> GetTriangles(Predicate<Triangle> trianglePredicate)
//{
//
//}
//
//

public List<TriangleMesh> Analyze()
{
List<TriangleMesh> meshes = new List<TriangleMesh>();
Expand Down
Loading

0 comments on commit 17e950b

Please sign in to comment.