I implemented a IMHO simple method:
- move to the assumed camera center
First you measure the 16 calibration points.
Use the 4 value for 0, 22.5, 45, 67.5, 90 and compute the intersection point of the line and its 90° orthogonal line.
Compute the Location average of the resulting 4 intersections
The gives you the true offset of the rotation center point to current location.
Subtract this offset to get the rotation center into the camera center
Repeat the measurement of the 16 calibration points.
These are now SUBTRACTED from the given XY setpoint.
Here are some code sniblets:
This is propably very different from Juhas code because I heavily reworked Juhas and therezas code.
I will support multi-head (intending to use 4), multi-nozzle with nozzle changer.
So when you move to a location you have to consider the individual head offset from down cam center and the the offset and runoff/wobble of the mounted nozzle.
Yes, I will publish the code when I am satisfied with my work.
Code: Select all
/// <summary>
/// Compute the inetrsection point of two lines A and B defined by their start and end loactions
/// </summary>
/// <param name="AStart"></param>
/// <param name="AEnd"></param>
/// <param name="BStart"></param>
/// <param name="BEnd"></param>
/// <returns></returns>
PartLocation LineIntersectionPoint(PartLocation AStart, PartLocation AEnd, PartLocation BStart, PartLocation BEnd)
{
// Get A,B,C of first line - points : ps1 to pe1
double A1 = AEnd.Y - AStart.Y;
double B1 = AStart.X - AEnd.X;
double C1 = A1 * AStart.X + B1 * AStart.Y;
// Get A,B,C of second line - points : ps2 to pe2
double A2 = BEnd.Y - BStart.Y;
double B2 = BStart.X - BEnd.X;
double C2 = A2 * BStart.X + B2 * BStart.Y;
// Get delta and check if the lines are parallel
double delta = A1 * B2 - A2 * B1;
if (delta == 0)
throw new Exception("Lines are parallel");
// now return the Vector2 intersection point
return new PartLocation(
(B2 * C1 - B1 * C2) / delta,
(A1 * C2 - A2 * C1) / delta
);
}
/// Computes the center intersection point of a nozzle calibration
/// list. This list contains 16 point measured in 22.5° steps.
/// Therefore we have 4 pairs of ortogonal intersecting lines
/// </summary>
public PartLocation ZCenterOffset
{
get
{
List<PartLocation> resList = new List<PartLocation>();
for (int i = 0; i < 4; i++)
{
PartLocation pResult = LineIntersectionPoint(AbsCalibrationPoints[i], AbsCalibrationPoints[i + 8], AbsCalibrationPoints[i + 4], AbsCalibrationPoints[i + 12]);
resList.Add(pResult);
}
return PartLocation.Average(resList);
}
}
/// <summary>
/// Calibrates the needle by first computing the real A axis center
/// and then the wobble offset
/// </summary>
/// <returns>true on success</returns>
public bool Calibrate()
{
// first we evaluate the true A-axis center point
if (Calibrate(4.0 / Config.Instance.UpCamXmmPerPixel))
{
// and we save it
Location currentCamCenter = LocationSet.FindLocation("UPCAMERA");
LocationSet.FindLocation("UPCAMERA").SetTo(currentCamCenter - ZCenterOffset);
CNC.MoveToLocation("UPCAMERA");
if (Calibrate(4.0 / Config.Instance.UpCamXmmPerPixel))
{
for (int i = 0; i < CalibrationPoints.Count; i++)
DashBoard.sLogText(2, CalibrationPoints[i].ToString(), Color.Purple);
return true;
}
}
MainForm.sShowError("Nozzle Calibration Failed!");
IsCalibrated = false;
return false;
}
/// <summary>
/// Add the specific nozzle offset and the runoff/wobble
/// induced by the current nozzle
///
/// </summary>
/// <param name="location"></param>
/// <returns></returns>
public SpaceLocation AddOffset(SpaceLocation location, bool subtract = false)
{
double dX = 0;
double dY = 0;
if (GetCorrectedPosition(location.A, out dX, out dY))
{
var wobble = new PartLocation(dX, dY, 0);
if (subtract)
{
location += wobble;
location -= Offset;
}
else
{
location -= wobble;
location += Offset;
}
}
return location;
}
And here the head Offset. When a nozzle is mounted on a head it's nozzle object reference is in Nozzle
Code: Select all
/// <summary>
/// When we move the machine to a head based location the designated location
/// in down-camera view is offset by the calibrated head + nozzle offsets.
/// </summary>
/// <param name="location">the space loaction X,Y,Z and Angle</param>
/// <returns></returns>
public SpaceLocation AddOffset(SpaceLocation location, bool subtract = false)
{
if (subtract)
location -= Offset;
else
location += Offset;
if (Nozzle != null)
location = Nozzle.AddOffset(location, subtract);
return location;
}
and finally in the center CNC
Code: Select all
public static bool MoveToXYAZ(double? x, double? y, double? angle = null, double? z = null, int? speed = null, OffsetStyle offsetStyle = OffsetStyle.Use_Default)
{
if (_ErrorOccured || !Connected)
{
LogText(2, string.Format("CNC.MoveToXYZA: move canceled because {0}", _ErrorOccured ? "Error has occured!" : "CNC not connected!"));
return false;
}
if (!CNC.JoggingBusy)
{
// if not set as parameter use global offset type
if (offsetStyle == OffsetStyle.Use_Default)
offsetStyle = OffsetType;
// depending on what we are intending to use add the appropriate offset
if (offsetStyle != OffsetStyle.Camera)
{
if (offsetStyle == OffsetStyle.Last_Head)
offsetStyle = _LastPnPHead;
SpaceLocation offset = NozzleSet.GetHeadByIndex((int)offsetStyle).AddOffset(new SpaceLocation(x, y, z, angle));
if (x != null) x = offset.X;
if (y != null) y = offset.Y;
if (z != null) z = offset.Z;
}
}
LogText(2, string.Format("MoveToXYZA: X:{0:#0.00} Y:{1:#0.00} Z:{2:#0.00} A:{3:#0.00}", x, y, z, angle), Color.DarkMagenta);
if (!_NoSafeTest && !CanMoveSafeTo(x, y, z))
{
LogText(2, "CNC.MoveToXYZA: ignored because cannot move safe!", Color.Red);
return false;
}
_BlockingWriteDone = false;
Thread t = new Thread(() => BlockingMoveThread(x, y, z, angle, speed))
{
Name = "CNC_BlockingMove",
IsBackground = true
};
t.Start();
int i = _MoveTimeout;
while (!_BlockingWriteDone && !_ErrorOccured)
{
Thread.Sleep(2);
Application.DoEvents();
i--;
if (i <= 0)
_readyEvent.Set(); // causes CNC_Blocking_thread to exit
}
_BlockingWriteDone = true;
if (((i > _MoveTimeout) || _ErrorOccured) && Connected)
{
MainForm.sShowError("CNC.MoveToXYZA: Timeout ");
//Close();
MainForm.sUpdateCNCStatus();
}
return (Connected);
. But assuming the ellipse shape is repeatable (i.e. the A axis is not precisely vertical so we are looking at a circle at an angle), our wobble compensation should be to the center of the ellipse and the cam to needle offset to that center. It takes multiple readings to characterize an ellipse, but I believe the center of the lines connecting two points 180 apart should pass through the center of the ellipse. The correction factor for every point should be relative to that point. If the ellipse is true (all lines from two 180 degree points find the same center), we should only need half of the measured points stored since the opposite point (180 degrees away) is mirrored across the center point.
). I have used other version control systems (CVS, SVN). Git is different, I haven't yet grokked it. I forked Juha's repository and cloned my repository to my local machine. I started making changes to that code (I probably should have created a branch). I am using Git Gui because Visual Studio asked for it. I expected Visual Studio to be integrated and do a check out. Something is not right because I have commits showing up in Git Gui, but not all the changes I have made are there. I haven't been able to see a diff to show my changes. I see the green and yellow bars in VS (which seem right). I don't yet visualize the path back to Juha's (or my) repository, making the changes available in them. I trying to work this out, we are also trying to get the placer working and a board we desperately need built. I had promised this board to the customer by Jul 4th and now I am late.
