|
|
using GummingEntity;
|
|
|
using System;
|
|
|
using System.Collections.Generic;
|
|
|
using System.Text;
|
|
|
|
|
|
namespace GummingSupport
|
|
|
{
|
|
|
public class Common
|
|
|
{
|
|
|
public static double PI = Math.PI;
|
|
|
|
|
|
public static int ToInt(string str)
|
|
|
{
|
|
|
int result = 0;
|
|
|
int.TryParse(str, out result);
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
public static uint ToUInt32(string str)
|
|
|
{
|
|
|
uint result = 0;
|
|
|
uint.TryParse(str, out result);
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
public static ushort ToUInt16(int? str)
|
|
|
{
|
|
|
ushort result = 0;
|
|
|
ushort.TryParse(str?.ToString(), out result);
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
public static double ToDouble(string str)
|
|
|
{
|
|
|
double result = 0;
|
|
|
double.TryParse(str, out result);
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
public static decimal ToDecimal(string str)
|
|
|
{
|
|
|
decimal result = 0;
|
|
|
decimal.TryParse(str, out result);
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
public static int Atoi(byte[] chr)
|
|
|
{
|
|
|
//return int.Parse(new string(chr));
|
|
|
return BitConverter.ToInt32(chr, 0);
|
|
|
}
|
|
|
|
|
|
public static double Acos(double value)
|
|
|
{
|
|
|
double getValue = Math.Acos(value);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
|
|
|
public static double Asin(double value)
|
|
|
{
|
|
|
double getValue = Math.Asin(value);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
|
|
|
public static double Sqrt(double value)
|
|
|
{
|
|
|
double getValue = Math.Sqrt(value);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
|
|
|
public static double Speed(double speed, double newSpeed)
|
|
|
{
|
|
|
if (double.IsNaN(newSpeed) || newSpeed == 0)
|
|
|
{
|
|
|
return speed;
|
|
|
}
|
|
|
return newSpeed;
|
|
|
}
|
|
|
|
|
|
public static double Pow(double x, double y)
|
|
|
{
|
|
|
double getValue = Math.Pow(x, y);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
|
|
|
public static double Sin(double value)
|
|
|
{
|
|
|
double getValue = Math.Sin(value);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
|
|
|
public static double Cos(double value)
|
|
|
{
|
|
|
double getValue = Math.Cos(value);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
public static double Atan2(double y, double x)
|
|
|
{
|
|
|
double getValue = Math.Atan2(y, x);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
public static double Atan(double value)
|
|
|
{
|
|
|
double getValue = Math.Atan(value);
|
|
|
if (double.IsNaN(getValue))
|
|
|
{
|
|
|
getValue = 0;
|
|
|
}
|
|
|
return getValue;
|
|
|
}
|
|
|
|
|
|
public static int Atoi(char[] chr)
|
|
|
{
|
|
|
return int.Parse(new string(chr));
|
|
|
}
|
|
|
|
|
|
public static Point SetEnd(Point pt, Point a)
|
|
|
{
|
|
|
Point b = new Point(a.x + pt.x, a.y + pt.y);
|
|
|
return b;
|
|
|
}
|
|
|
|
|
|
public static int Bitnum(uint num, int bit)
|
|
|
{
|
|
|
uint value = num >> bit & 0x01;
|
|
|
if (value == 1)
|
|
|
return 1;
|
|
|
else
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
public static bool Getend(byte a)
|
|
|
{
|
|
|
return !(Bitnum(a, 7) == 1 && Bitnum(a, 6) == 1 && Bitnum(a, 5) == 1 && Bitnum(a, 4) == 1 && Bitnum(a, 3) == 0 && Bitnum(a, 2) == 0 && Bitnum(a, 1) == 1 && Bitnum(a, 0) == 1);
|
|
|
}
|
|
|
|
|
|
public static int GetmoveY(byte a, byte b, byte c)
|
|
|
{
|
|
|
int move_y;
|
|
|
move_y = 81 * Bitnum(a, 5) - 81 * Bitnum(a, 4)
|
|
|
+ 3 * Bitnum(b, 7) - 3 * Bitnum(b, 6) + 27 * Bitnum(b, 5) - 27 * Bitnum(b, 4)
|
|
|
+ 1 * Bitnum(c, 7) - 1 * Bitnum(c, 6) + 9 * Bitnum(c, 5) - 9 * Bitnum(c, 4);
|
|
|
return move_y;
|
|
|
}
|
|
|
|
|
|
public static int GetmoveX(byte a, byte b, byte c)
|
|
|
{
|
|
|
int move_x;
|
|
|
move_x = 81 * Bitnum(a, 2) - 81 * Bitnum(a, 3)
|
|
|
+ 3 * Bitnum(b, 0) - 3 * Bitnum(b, 1) + 27 * Bitnum(b, 2) - 27 * Bitnum(b, 3)
|
|
|
+ 1 * Bitnum(c, 0) - 1 * Bitnum(c, 1) + 9 * Bitnum(c, 2) - 9 * Bitnum(c, 3);
|
|
|
return move_x;
|
|
|
}
|
|
|
|
|
|
//200926根据斜率、针距和合速度（RPM）计算脉冲合速度（P/MS）。之前XY轴共用当量，当量不一致时会导致XY方向针距不均匀，正圆弧针距周期性变化。
|
|
|
public static double Getvel(double zhuansu, double zhenju, double yuxianjiao, double xdang, double ydang)
|
|
|
{
|
|
|
double Vxy = zhuansu * zhenju / 60.0;//rpm-mm/s
|
|
|
double Vx = Vxy * Math.Cos(yuxianjiao);//mm/s
|
|
|
double Vy = Vxy * Math.Sin(yuxianjiao);//mm/s
|
|
|
double Vxyp = Math.Sqrt(Math.Pow(Vx * xdang, 2) + Math.Pow(Vy * ydang, 2)) / 1000.0;//p/ms
|
|
|
return Vxyp;
|
|
|
}
|
|
|
|
|
|
public static byte[] HexStringToByteArray(string s)
|
|
|
{
|
|
|
s = s.Replace(" ", "");
|
|
|
|
|
|
byte[] buffer = new byte[s.Length / 2];
|
|
|
|
|
|
for (int i = 0; i < s.Length; i += 2)
|
|
|
|
|
|
buffer[i / 2] = (byte)Convert.ToByte(s.Substring(i, 2), 16);
|
|
|
|
|
|
return buffer;
|
|
|
|
|
|
}
|
|
|
|
|
|
public static string ByteArrayToHexString(byte[] data)
|
|
|
{
|
|
|
StringBuilder sb = new StringBuilder(data.Length * 3);
|
|
|
|
|
|
foreach (byte b in data)
|
|
|
|
|
|
sb.Append(Convert.ToString(b, 16).PadLeft(2, '0').PadRight(3, ' '));
|
|
|
|
|
|
return sb.ToString().ToUpper();
|
|
|
}
|
|
|
|
|
|
public static double SetAcc(int inacc, double xyscale)
|
|
|
{
|
|
|
double acc = inacc / 1000000.0;
|
|
|
return acc * xyscale;// XYdangScale;
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
/// 线段与圆的交点
|
|
|
/// </summary>
|
|
|
/// <param name="ptStart">线段起点</param>
|
|
|
/// <param name="ptEnd">线段终点</param>
|
|
|
/// <param name="ptCenter">圆心坐标</param>
|
|
|
/// <param name="Radius2">圆半径平方</param>
|
|
|
/// <param name="ptInter1">交点1(若不存在返回65536)</param>
|
|
|
/// <param name="ptInter2">交点2(若不存在返回65536)</param>
|
|
|
public static bool LineInterCircle(Point ptStart, Point ptEnd, Point ptCenter, double Radius2,
|
|
|
ref Point ptInter1, ref Point ptInter2)
|
|
|
{
|
|
|
double EPS = 0.00001;
|
|
|
ptInter1.x = ptInter2.x = 65536.0f;
|
|
|
ptInter2.y = ptInter2.y = 65536.0f;
|
|
|
float fDis = (float)Math.Sqrt((ptEnd.x - ptStart.x) * (ptEnd.x - ptStart.x) + (ptEnd.y - ptStart.y) * (ptEnd.y - ptStart.y));
|
|
|
Point d = new Point();
|
|
|
d.x = (ptEnd.x - ptStart.x) / fDis;
|
|
|
d.y = (ptEnd.y - ptStart.y) / fDis;
|
|
|
Point E = new Point();
|
|
|
E.x = ptCenter.x - ptStart.x;
|
|
|
E.y = ptCenter.y - ptStart.y;
|
|
|
double a = E.x * d.x + E.y * d.y;
|
|
|
double a2 = a * a;
|
|
|
double e2 = E.x * E.x + E.y * E.y;
|
|
|
if ((Radius2 - e2 + a2) < 0)
|
|
|
{
|
|
|
return false;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
double f = (float)Math.Sqrt(Radius2 - e2 + a2);
|
|
|
double t = a - f;
|
|
|
if (((t - 0.0) > -EPS) && (t - fDis) < EPS)
|
|
|
{
|
|
|
ptInter1.x = ptStart.x + t * d.x;
|
|
|
ptInter1.y = ptStart.y + t * d.y;
|
|
|
}
|
|
|
t = a + f;
|
|
|
if (((t - 0.0) > -EPS) && (t - fDis) < EPS)
|
|
|
{
|
|
|
ptInter2.x = ptStart.x + t * d.x;
|
|
|
ptInter2.y = ptStart.y + t * d.y;
|
|
|
}
|
|
|
return true;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
public static double GetAngle(double startPointX, double startPointY, double centerPointX, double centerPointY, double endPointX, double endPointY)
|
|
|
{
|
|
|
double acosValue = ((endPointX - centerPointX) * (startPointX - centerPointX) +
|
|
|
(endPointY - centerPointY) * (startPointY - centerPointY))
|
|
|
/ Common.Sqrt(Common.Pow((endPointX - centerPointX), 2) + Common.Pow((endPointY - centerPointY), 2))
|
|
|
/ Common.Sqrt(Common.Pow((startPointX - centerPointX), 2) + Common.Pow((startPointY - centerPointY), 2));
|
|
|
if (acosValue < -1)
|
|
|
{
|
|
|
acosValue = -1;
|
|
|
}
|
|
|
if (acosValue > 1)
|
|
|
{
|
|
|
acosValue = 1;
|
|
|
}
|
|
|
return Acos(acosValue) * 180.0 / Math.PI;
|
|
|
}
|
|
|
|
|
|
public static double GetAngle(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
|
|
|
{
|
|
|
// 线段的向量表示形式
|
|
|
double v1x = x2 - x1, v1y = y2 - y1;
|
|
|
double v2x = x4 - x3, v2y = y4 - y3;
|
|
|
|
|
|
// 向量点积
|
|
|
double dotProduct = v1x * v2x + v1y * v2y;
|
|
|
|
|
|
// 向量模长
|
|
|
double v1Length = Common.Sqrt(v1x * v1x + v1y * v1y);
|
|
|
double v2Length = Common.Sqrt(v2x * v2x + v2y * v2y);
|
|
|
|
|
|
// 计算夹角的余弦值
|
|
|
double cosAngle = dotProduct / (v1Length * v2Length);
|
|
|
|
|
|
// 计算夹角的角度值
|
|
|
return Acos(cosAngle) * 180.0 / Math.PI;
|
|
|
}
|
|
|
}
|
|
|
}
|
