Chương trình c điều khiển máy CNC 3 trục

P.1. Code Software trên C# chường trình điều khiểm máy CNC 3D
using System;
using System.Windows.Forms;
using System.IO.Ports;
using System.IO;
using System.Media;
namespace giaodiendktd
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
int intlen = 0; // luu gia tri cong com ket noi vao may tinh
int m;
string trucX; string trucY; string trucZ; string lapthuN;//(thông số gửi cho VĐK)
public SoundPlayer baoloi = new SoundPlayer("baodong.wav");
public SoundPlayer ketthuc = new SoundPlayer("hoanthanh.wav");
private void groupBox1_Enter(object sender, EventArgs e)
{
}
private void groupBox3_Enter(object sender, EventArgs e)
{
}
private void button2_Click(object sender, EventArgs e)
{
TTA1.Clear(); TTA2.Clear(); TTA3.Clear();
TTB1.Clear(); TTB2.Clear(); TTB3.Clear();
txtnhan.Clear(); TXTF.Clear();
}

private void label1_Click(object sender, EventArgs e)
{
}
private void label3_Click(object sender, EventArgs e)
{
}
private void button3_Click(object sender, EventArgs e)
{
}
private void groupBox4_Enter(object sender, EventArgs e)
{
}
private void Pbconnect_Click(object sender, EventArgs e)
{


if (LbStatus.Text == "Disconnect")
{
Com.PortName = CbSecCom.Text;
Com.Open();
LbStatus.Text = "Connect";
Pbconnect.Text = "Disconnect";
}
else
{
Com.Close();
LbStatus.Text = "Disconnect";
Pbconnect.Text = "Connect";
}
}

int tdo;
private void timer1_Tick(object sender, EventArgs e)
{
String[] ports = SerialPort.GetPortNames();
if (intlen != ports.Length)
{
intlen = ports.Length;
CbSecCom.Items.Clear();
for (int j = 0; j < intlen; j++)
{
CbSecCom.Items.Add(ports[j]);
}
}
tdo = tocdo.Value;
// vtx.Text = tdo.ToString();
}
private void CbSecCom_SelectedIndexChanged(object sender, EventArgs e)
{
}
private void guitt_Click(object sender, EventArgs e)
{
if (LbStatus.Text == "Connect")
{
if (TTA1.Text == "" || TTA2.Text == "" || TTA3.Text == "" || TTB1.Text == "" || TTB2.Text
== "" || TTB3.Text == "" || TXTF.Text == "")
{ MessageBox.Show("Chưa có dữ liệu!", "Thông Báo"); }
else
{
float a = float.Parse(TTA1.Text);
float b = float.Parse(TTA2.Text);

float c = float.Parse(TTA3.Text);
float A = float.Parse(TTB1.Text);
float B = float.Parse(TTB2.Text);
float C = float.Parse(TTB3.Text);


float F = float.Parse(TXTF.Text);
noisuytuyentinh(a, b, c, A, B, C,F);
}
}
else
{
MessageBox.Show("COM chưa mở.", "Thông báo", MessageBoxButtons.OK,
MessageBoxIcon.Error);
}
}
private void TTA1_TextChanged(object sender, EventArgs e)
{
}
private void TTA2_TextChanged(object sender, EventArgs e)
{
}
// ***** Gửi dữ liệu nội suy cung tròn*************
private void guict_Click(object sender, EventArgs e)
{
if (LbStatus.Text == "Connect")
{
if (CTA1.Text == "" || CTA2.Text == "" || CTB1.Text == "" || CTB2.Text == "")
{ MessageBox.Show("Chưa có dữ liệu!", "Thông Báo"); }
else

{
float a = float.Parse(CTA1.Text);
float b = float.Parse(CTA2.Text);
float A = float.Parse(CTB1.Text);
float B = float.Parse(CTB2.Text);
noisuycungtron(a, b, A, B, y);
}
}
else
{
MessageBox.Show("COM chưa mở.", "Thông báo", MessageBoxButtons.OK,
MessageBoxIcon.Error);
}
}
//********************** Xóa dữ liệu cung tròn*************
private void xoact_Click(object sender, EventArgs e)
{
baoloi.Play();
}
private void xoadt_Click(object sender, EventArgs e)
{
txtnhan.Clear();
}
private void guidt_Click(object sender, EventArgs e)
{


string q;
if (LbStatus.Text == "Connect")
{

if (bluZ.Text == "" || bluX.Text == "" || bluY.Text == "" )
{ MessageBox.Show("Chưa có dữ liệu!", "Thông Báo"); }
else
{
q = bluZ.Text;
Com.WriteLine(q);
q = bluX.Text;
Com.WriteLine(q);
q = bluY.Text;
Com.WriteLine(q);
}
}
else
{
MessageBox.Show("COM chưa mở.", "Thông báo", MessageBoxButtons.OK,
MessageBoxIcon.Error);
}
}
private void label8_Click(object sender, EventArgs e)
{
}
//**********************************************************
Action<string> comaction;
private void oncom(object sender, SerialDataReceivedEventArgs e)
{
comaction = nhan;
try
{
this.BeginInvoke(comaction, Com.ReadExisting());
}

catch { }
}
//***************** Chương trình tính căn bạc 2 *******************
// Chuong trinh tinh can trong vi dieu khien
float canbachai(float a)
{
float x, i, y;
x = 1;
for (i = 1; i <= 8; i++)
{
y = x;
x = (a / y + y) / 2;
}
return x;


}
// ************** Nội Suy Cung Tròn ******************************
private void noisuycungtron(float X0, float Y0, float Xf, float Yf, string chieu)
{
float BLU = 1;
float[] mangx = new float[1000];
float[] mangy = new float[1000];
float Axf,Ayf,Ax,Ay,D;
int dem1 = 0;
if (chieu == "1") { mangy[0] = Y0; mangx[0] = X0; }
if (chieu == "2") { mangy[0] = Yf; mangx[0] = Xf; }
//////////////////////////////////////////////////////////////////////
if (X0 >= Xf) { Axf = X0; } else Axf = Xf;
if (Y0 >= Yf) { Ayf = Y0; } else Ayf = Yf;

Ax = 2 * X0 + 1; Ay = -2 * Y0 + 1; D = 0;
do
{
if (D > 0)
{
D = D + Ay; Ay = Ay + 2; Ayf = Ayf - 1;
if (chieu == "1")
{
mangy[dem1 + 1] = mangy[dem1] - BLU;
mangx[dem1 + 1] = mangx[dem1]; dem1++;
}
if (chieu == "2")
{
mangy[dem1 + 1] = mangy[dem1];
mangx[dem1 + 1] = mangx[dem1] - BLU; dem1++;
}
}
if (D <= 0)
{
D = D + Ax; Ax = Ax + 2; Axf = Axf - 1;
if (chieu == "1")
{
mangy[dem1 + 1] = mangy[dem1];
mangx[dem1 + 1] = mangx[dem1] + BLU; dem1++;
}
if (chieu == "2")
{
mangy[dem1 + 1] = mangy[dem1] + BLU;
mangx[dem1 + 1] = mangx[dem1]; dem1++;
}

}
} while (Ayf != 0);
noisuyx.Clear(); noisuyy.Clear(); noisuyz.Clear(); STT.Clear();
for (int j = 0; j < dem1; j++)


{
STT.Text += j + "\r\n";
noisuyx.Text += mangx[j] + "\r\n";
noisuyy.Text += mangy[j] + "\r\n";
}
string[] xungx = { "X+123", "X-2048", "X+6144", "X+0", "X+0", "X-0" };
string[] xungy = { "Y-456", "Y+4096", "Y-4096", "Y+0", "Y+0", "Y+0" };
string[] xungz = { "Z-78", "Z-8192", "Z+0", "Z+2049", "Z+2052", "Z+2055" };
trucX = xungx[net];
trucY = xungy[net];
trucZ = xungz[net];
Com.Write("?");
//************************ Nội Suy tuyến tính********************
private void noisuytuyentinh(float a1, float a2, float a3, float b1,float b2, float b3,float f)
{
float Tipo = 1;
// do dai duong thang AB
float L=canbachai((b1-a1)*(b1-a1)+(b2-a2)*(b2-a2)+(b3-a3)*(b3-a3));
float Al = f/60;// độ dài phep nội suy
float T = (L /(f / 60));
// so lan lap phep noi suy
float N = T / Tipo;
int n = (int)N;
// Do dai dich chuyen cua moi truc

float Ax=(Al/L)*(b1-a1);
float Ay=(Al/L)*(b2-a2);
float Az=(Al/L)*(b3-a3);
// Noi suy tuyen tinh
float[] MA = new float [n];
float[] MB = new float[n];
float[] MC = new float[n];
float A=a1; float B=a2; float C=a3;
for(int i=1;i<=n;i++){
A=A+Ax;
B=B+Ay;
C=C+Az;
MA[i - 1] = A;
MB[i - 1] = B;
MC[i - 1] = C;
}
noisuyx.Clear(); noisuyy.Clear(); noisuyz.Clear(); STT.Clear();
for (int j = 0; j <= n-1; j++)
{
STT.Text += j+1 + "\r\n";
noisuyx.Text += MA[j] + "\r\n";
noisuyy.Text += MB[j] + "\r\n";
noisuyz.Text += MC[j] + "\r\n";


}
string[] xungx = { "X-8", "X+3200","X+6144", "X+0", "X+0", "X-0" };
string[] xungy = { "Y+6", "Y+3200","Y-4096", "Y+0", "Y+0", "Y+0" };
string[] xungz = { "Z-7", "Z+3200","Z+0", "Z+2049", "Z+2052", "Z+2055" };
lapthuN = "L100";

trucX = xungx[net];
trucY = xungy[net];
trucZ = xungz[net];
Com.Write("?");
}
int E;
// ***************** hàm tách dữ lieu ******************************
private void chiadulieu(int i, string M)
{
char[] mangkytu;
//chuyển chuỗi thành mảng ký tự
mangkytu = M.ToCharArray();
E = mangkytu.Length;// so phan tu trong mang
//duyệt từng ký tự trong mảng
if (i < E)
{
char ch = mangkytu[i]; //đọc ký tự thứ i và gán vào biến ch
string linh = ch.ToString(); //ep char sang string
Com.Write(linh);
}
if (G > E)
{
Com.Write("h");
G = 1;} }
//************************* ham su ly du lieu nhan ve ********************
string s; string c; int G = 1; int it; string rt; int kiemtra=0,kta=0,kj=0,xacdinhmag;
private void nhan(string input)
{
txtnhan.Text += input;
c = input;

if (c == "*") { m++; c = "1"; }
if (c == "c") { Com.Write("G"); }
if (c == "#")
{
m = 0; if (kiemtra != tdo) { Com.Write("["); }
else
{
if (xacdinhmag == 1) { c = "&"; }
if (xacdinhmag == 2) { c = ">"; }
}
}
if (c == "/") { G++; ; c = "1"; }


if (c == "&") { kj++; m = 0; dem++; truyengcode(); if (kj == 3) { txtnhan.Clear();
kj = 0; } }
if (c == "B") { txtnhan.Clear(); MessageBox.Show("Đã kết nối thành công thiết bị
với Software..."); }
if (c == "!") { m = 6; }
if (c == "x") { TDUNG = 1; baoloi.Play(); baoloi.Play(); MessageBox.Show("Quá
Giới Hạn Hành Trình TrucX"); }
if (c == "y") { TDUNG = 1; baoloi.Play(); baoloi.Play(); MessageBox.Show("Quá
Giới Hạn Hành Trình TrucY"); }
if (c == "z") { TDUNG = 1; baoloi.Play(); baoloi.Play(); MessageBox.Show("Quá
Giới Hạn Hành Trình TrucZ"); }
if (c == "]")
{
kiemtra = tdo;
kta = 300 - tdo;
m = 7;

rt = tdo.ToString();
}
if (c == ">")
{
kj++;
if (it <= tangdem) { Com.Write(CT[it]); }
if (it > tangdem) { Com.Write("g"); it = 1; tangdem = 1; }
it++; vtz.Text = it.ToString();
if (kj == 30) { txtnhan.Clear(); kj = 0; }
}
if (m == 1)
{
s = trucX;
chiadulieu(G - 1, s);
}
if (m == 2)
{
s = trucY;
chiadulieu(G - 1, s);
}
if (m == 3)
{
s = trucZ;
chiadulieu(G - 1, s);
}
if (m == 4)
{
s = lapthuN;
chiadulieu(G - 1, s);
}

if (m == 5)
{


s = "S";
Com.Write(s);
}
if (m == 6)
{
s = SS.ToString();
chiadulieu(G - 1, s);
}
if (m == 7)
{
s = rt;
chiadulieu(G - 1, s);
}
}
//****************** ham kiểm ta chieu quay **********************
string y;
private void CW_CheckedChanged(object sender, EventArgs e)
{
y = "1";
}
private void CCW_CheckedChanged(object sender, EventArgs e)
{
y = "2";
// MessageBox.Show("nguoc chieu kim dong ho "+y);
}
#region di chuyen tro

public void fcus(int vitri)
{
switch (vitri)
{
case 1:
{
TTA1.Focus();
break;
}
case 2:
{
TTA2.Focus();
break;
}
case 3:
{
TTA3.Focus();
break;
}
case 4:
{
TTB1.Focus();


break;
}
case 5:
{
TTB2.Focus();
break;

}
case 6:
{
TTB3.Focus();
break;
}
case 7:
{
TXTF.Focus();
break;
}
case 8:
{
CTA1.Focus();
break;
}
case 9:
{
CTA2.Focus();
break;
}
case 10:
{
CTB1.Focus();
break;
}
case 11:
{
CTB2.Focus();
break;

}
}
}
private void TTB1_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39 )
{
fcus(5);
}
}
private void TTA1_KeyDown(object sender, KeyEventArgs e)
{


if (e.KeyValue == 39)
{
fcus(2);
}
}
private void TTA2_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(3);
}
}
private void TTA3_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{

fcus(4);
}
}
private void TTB2_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(6);
}
}
private void TTB3_TextChanged(object sender, EventArgs e)
{
}
private void TTB3_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(7);
}
}
private void TXTF_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(8);
}
}
private void CTA1_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)

{


fcus(9);
}
}
private void CTA2_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(10);
}
}
private void CTB1_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(11);
}
}
private void CTB2_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyValue == 39)
{
fcus(1);
}
}
#endregion
private void txtnhan_TextChanged(object sender, EventArgs e)
{

}
private void textBox1_TextChanged(object sender, EventArgs e)
{
}
private void groupBox6_Enter(object sender, EventArgs e)
{
}
private void Form1_Load(object sender, EventArgs e)
{
}
private void label13_Click(object sender, EventArgs e)
{
}
private void groupBox2_Enter(object sender, EventArgs e)
{
}
private void TTA3_TextChanged(object sender, EventArgs e)
{
}
private void label2_Click(object sender, EventArgs e)
{


}
private void TTB1_TextChanged(object sender, EventArgs e)
{
}
private void TTB2_TextChanged(object sender, EventArgs e)
{
}

private void TXTF_TextChanged(object sender, EventArgs e)
{
}
private void label14_Click(object sender, EventArgs e)
{
}
private void TXTBLU_TextChanged(object sender, EventArgs e)
{
}
private void tamy_TextChanged(object sender, EventArgs e)
{
}
int net=0;
private void next_Click(object sender, EventArgs e)
{
STTxung.Clear();
net++;
STTxung.Text += net;
if (net == 6) { net = 0; }
}
// ****************** ham reset vị trí ban đầu ***********************
private void Reset_CheckedChanged(object sender, EventArgs e)
{ }
//************** Ham xu ly vi tri hien tai dong co***************
private void bluZ_TextChanged(object sender, EventArgs e)
{
}
private void groupBox8_Enter(object sender, EventArgs e)
{
}

private void tam_Click(object sender, EventArgs e)
{
}
private void STTxung_TextChanged(object sender, EventArgs e)
{
}
OpenFileDialog gcode = new OpenFileDialog();
private void btnopen_Click(object sender, EventArgs e)
{
gcode.Title="OPEN GCODE";
gcode.InitialDirectory = @"Desktop";


gcode.Filter = "Gcode|*.nc";
if (gcode.ShowDialog() == DialogResult.OK)
{
txtpath.Text = gcode.FileName;
magcode.Text = File.ReadAllText(txtpath.Text);
dem = 0;
}
}
private void textBox7_TextChanged(object sender, EventArgs e)
{
}
private void label18_Click(object sender, EventArgs e)
{
private void label16_Click(object sender, EventArgs e)
{
}
private void groupBox14_Enter(object sender, EventArgs e)

{
}
private void button3_Click_1(object sender, EventArgs e)
{
magcode.Clear();
}
private void luugcode_Click(object sender, EventArgs e)
{
StreamWriter write = new StreamWriter(txtpath.Text,true);
write.WriteLine(magcode.Text);
write.Close();
}
// chuyen file gcode thành đọc từng hàng ghép vào textbox xulygcode
int i, dem = 1, dems1 = 0;
private void docgcode(int solan)
{
StreamReader file = new StreamReader(txtpath.Text,false);
for(i=0;i{
string G = file.ReadLine();
ll: if (G.Trim() != "") { xulygcode.Text = G; tachcode(G); dems1 = 0; }
if (G.Trim() == "")
{
G = file.ReadLine();
if (dems1 < 10) { dems1++; goto ll; }
if (dems1 == 10) { xulygcode.Text = "End;"; dem = 1; TDUNG = 1;
MessageBox.Show("Hoàn Thành Đọc Gcode...!"); break; }// reset lai dem và thoát
}} }
int ij;
private void tachcode(string code)

{
string malenh = "";
string matoado = "";
int nho = 3;
char[] mangcode;
mangcode = code.ToCharArray();
for (ij = 0; ij < code.Length; ij++)
{
if (mangcode[ij] == ' ') { nho = ij; break; } // xác địn vị trí tọa độ sau mã lệnh
ngăn cách bằng khoản trắng
if (mangcode[ij] == 'F') { nho = ij + 1; matoado = matoado + mangcode[ij+1];
break; } // đọc thấy lượng ăn dao
}
for (ij = 0; ij < nho; ij++)// ghep mã lệnh
{
malenh = malenh+mangcode[ij].ToString();// ghép mã lệnh
}
lenh.Clear();// xóa mã lệnh trước
lenh.Text = malenh;// hiển thị mã lệnh lên màng hình
for (ij = nho+1; ij {
matoado = matoado + mangcode[ij].ToString();// ghép tọa độ
}
toado.Clear();// xóa tọa độ gốc
toado.Text = matoado;// hiển thị tọa độ mới
if (malenh != "S") { tachtoado(matoado); } // chuyển tọa độ đi tách thành tọa độ các
trục
}

private void magcode_TextChanged(object sender, EventArgs e)
{ }
// *****************Hàm tách tọa độ thành tọa độ các trục***************
private void tachtoado(string td)
{
int it,hi=0;
int nhox=0,nhoy=0,nhoi=0,demtd=0;
string gtx = "", gty = "", gti = "", gtj = "",Rb="";// tọa độ điểm cuối hoặc tọa đô tâm
char[]tdo;
tdo = td.ToCharArray();
for (it = 0; it < td.Length; it++)
{
if (tdo[it] == ' ')
{
demtd++; if (it == 0 && demtd == 1) { demtd--; hi = 1; }
{
if (demtd == 1) { nhox = it; }
if (demtd == 2) { nhoy = it; }
if (demtd == 3) { nhoi = it; }
}


}
}
////////////////////////
if (demtd == 0&&td.Length!=0) { TXTF.Text = td; }// tách mã F
///////////////////// tach toa do của dierm cuuoi
if (demtd == 1)
{
for (it = 1+hi; it < nhox; it++)

{
gtx=gtx+tdo[it];// tọa độ trục x
}
for (it = nhox+2; it {
gty = gty + tdo[it];// tọa độ truc y
}
}
//////////////////////////////////////////////
if (demtd == 2)
{
for (it = 1 + hi; it < nhox; it++)
{
gtx = gtx + tdo[it];// tọa độ trục x
}
for (it = nhox + 2; it < nhoy; it++)
{
gty = gty + tdo[it];// tọa độ truc y
}
for (it = nhoy + 2; it < td.Length; it++)
{
Rb = Rb + tdo[it];// ban kinh
}
}
////////////////////////////////////////////////
if (demtd==3)// tach toa do dierm tam
{
for (it = 1; it < nhox; it++)
{
gtx = gtx + tdo[it];// toa dộ x

}
for (it = nhox + 2; it < nhoy; it++)
{
gty = gty + tdo[it];// toa dộ y
}
for (it = nhoy + 2; it < nhoi; it++)
{
gti = gti + tdo[it];// toa dộ i
}
for (it = nhoi + 2; it < td.Length; it++)


{
gtj = gtj + tdo[it];// toa dộ j
}
}
/////////////////////////
if (demtd==1)// hiển thị tọa độ ra ô text box
{
TTB1.Text = gtx;
TTB2.Text = gty;
}
if (demtd == 2)
{
CTA1.Text = gtx;
CTA2.Text = gty;
CTB2.Text = Rb;
}
if (demtd == 3)// hiển thị tọa độ ra ô text box
{

CTA1.Text = gtx;
CTA2.Text = gty;
CTB1.Text = gti;
CTB2.Text = gtj;
}
}
private void groupBox13_Enter(object sender, EventArgs e)
{
}
private void vmx_TextChanged(object sender, EventArgs e)
{
}
private void button5_Click(object sender, EventArgs e)
{
vmx.Text = "8.0";
vmy.Text = "8.0";
vmz.Text = "8.0";
stx.Text = "1.8";
sty.Text = "1.8";
stz.Text = "1.8";
driverx.Text ="16.0";
drivery.Text = "16.0";
driverz.Text = "16.0";
float vmxf = float.Parse(vmx.Text);
float vmyf = float.Parse(vmy.Text);
float vmzf = float.Parse(vmz.Text);
float stxf = float.Parse(stx.Text);
float styf = float.Parse(sty.Text);
float stzf = float.Parse(stz.Text);
float driverxf = float.Parse(driverx.Text);

float driveryf = float.Parse(drivery.Text);
float driverzf = float.Parse(driverz.Text);
minstepx.Text = ((vmxf/360F)*(stxf/driverxf)).ToString();
minstepy.Text = ((vmyf /360F) * (styf / driveryf)).ToString();
minstepz.Text = ((vmzf/360F) * (stzf / driverzf)).ToString();
}
private void minstepx_TextChanged(object sender, EventArgs e)
{
}
////////////////////////////////////////////////////////////////////////////////////////////////
int chieucao;
int chieusau;
private void button4_Click(object sender, EventArgs e)
{
float vmxf = float.Parse(vmx.Text);
float vmyf = float.Parse(vmy.Text);
float vmzf = float.Parse(vmz.Text);
float stxf = float.Parse(stx.Text);
float styf = float.Parse(sty.Text);
float stzf = float.Parse(stz.Text);
float driverxf = float.Parse(driverx.Text);
float driveryf = float.Parse(drivery.Text);
float driverzf = float.Parse(driverz.Text);
minstepx.Text = ((vmxf / 360F) * (stxf / driverxf)).ToString();
minstepy.Text = ((vmyf / 360F) * (styf / driveryf)).ToString();
minstepz.Text = ((vmzf / 360F) * (stzf / driverzf)).ToString();
// Dich chuyen truc Z
float ccp = float.Parse(CCP.Text) / float.Parse(minstepx.Text);

chieucao = (int)ccp+1600;
float csc = float.Parse(CSC.Text) / float.Parse(minstepx.Text);
chieusau = (int)csc+1600;
// chằn tọa độ x,y,z, vào
trucX = "X+0";
trucY = "Y+0";
trucZ = "Z+" + chieucao.ToString();
lapthuN = "L1";
Com.Write("?");
}
// Hàm di chuyển dao đến 1 vị trí mong muốn theo duong thang chay không tải nhất
dao lên và chạy
private void G00()
{
float x = float.Parse(TTA1.Text);
float y = float.Parse(TTA2.Text);
float z = float.Parse(TTA3.Text);
float a = float.Parse(TTB1.Text);
float b = float.Parse(TTB2.Text);


float c = float.Parse(TTB3.Text);
float Tipo;
float Vf = 10; // chương trinh offset
// do dai duong thang AB
double L = Math.Sqrt(Math.Abs(a - x) * Math.Abs(a - x) + Math.Abs(b - y) *
Math.Abs(b - y) + Math.Abs(c - z) * Math.Abs(c - z));
gocL.Text = L.ToString();
Tipo = (float.Parse(V.Text) / 60f) / float.Parse(minstepx.Text);
Tipo = (1/Tipo);

// do dai phep noi suy
float F = float.Parse(V.Text);
float AL = Vf*(F/60F) * (Tipo);
nhom.Text = AL.ToString();
// Do dai dich chuyen cua moi truc
double Ax = (AL / L) * (a - x);
double Ay = (AL / L) * (b - y);
double Az = (AL / L) * (c - z);
gocX.Text = Ax.ToString();
gocY.Text = Ay.ToString();
gocZ.Text = Az.ToString();
// So vong lap noi suy N
double T = L / (Vf*F / 60F);
double N1 = (T /Tipo);
int N = (int)N1;
// Gửi dữ liệu đi tính xung cho mỗi trục động cơ
tinhxung(Ax, Ay, Az, N);
}
//Hàm tính xung cho mỗi trục sau nội suy
private void tinhxung(double x1, double y1, double z1, int lanlapN)
{
int nhox = 0;
int nhoy = 0;
double thieux = 0;
double thieuy = 0;
int solanthieux = 0;
int solanthieuy = 0;
float minx = float.Parse(minstepx.Text);
float miny = float.Parse(minstepy.Text);
float minz = float.Parse(minstepz.Text);

double ttx = (x1 / minx); if (ttx < 0) { ttx = Math.Abs(ttx); nhox = 1; } int ttxn =
(int)ttx;
double tty = (y1 / miny); if (tty < 0) { tty = Math.Abs(tty); nhoy = 1; } int ttyn =
(int)tty;
// double ttz = (z1 / minz); if (ttz < 0) { ttz = Math.Abs(ttz); nhoz = 1; } int ttzn =
(int)ttz;
if(ttxn>0){ solanthieux=lanlapN/ttxn;}
if(ttyn>0){solanthieuy = lanlapN / ttyn;}
thieux = (ttx - ttxn) * solanthieux;


// vtx.Text = thieux.ToString();
thieuy = (tty - ttyn) * solanthieuy;
// vtx.Text = thieuy.ToString();
if (thieux >= thieuy) { lanlapN = (lanlapN + (int)thieux); }
if (thieuy > thieux) { lanlapN = (lanlapN + (int)thieuy); }
// chằn tọa độ x,y,z, vào
if (nhox==0) { trucX = "X+" + ttxn.ToString(); }
if (nhox==1) { trucX = "X-" + ttxn.ToString(); }
if (nhoy==0) { trucY = "Y+" + ttyn.ToString(); }
if (nhoy==1) { trucY = "Y-" + ttyn.ToString(); }
// if (nhoz==0) { trucZ = "Z+" + ttzn.ToString(); }
// if (nhoz==1) { trucZ = "Z-" + ttzn.ToString(); }
if ((xd0==1)&&(xd1==0)) { trucZ = "Z+" + chieusau.ToString(); }
if ((xd0 == 0) && (xd1 == 1)) { trucZ = "Z-" + chieusau.ToString(); }
if ((xd0 != 1) && (xd1 == 0)) { trucZ = "Z+0"; }
if ((xd0 == 0) && (xd1 != 1)) { trucZ = "Z-0"; }
lapthuN = "L"+lanlapN.ToString();
// truyen dữ liệu ra màng hình
bluX.Text = trucX;

bluY.Text = trucY;
bluZ.Text = trucZ;
vonglap.Text = lapthuN;
xacdinhmag = 1;
Com.Write("?");// gửi dữ liệu đến vi điều khiển
}
/// <summary>
/// /////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// </summary>
private void G01()
{
float x = float.Parse(TTA1.Text);
float y = float.Parse(TTA2.Text);
float z = float.Parse(TTA3.Text);
float a = float.Parse(TTB1.Text);
float b = float.Parse(TTB2.Text);
float c = float.Parse(TTB3.Text);
float Tipo;
float Vf = 10; // chương trinh offset
// do dai duong thang AB
double L = Math.Sqrt(Math.Abs(a - x) * Math.Abs(a - x) + Math.Abs(b - y) *
Math.Abs(b - y) + Math.Abs(c - z) * Math.Abs(c - z));
gocL.Text = L.ToString();
Tipo = (float.Parse(V.Text) / 60f) / float.Parse(minstepx.Text);
Tipo = (1 / Tipo);
// do dai phep noi suy
float F = float.Parse(TXTF.Text);
float AL = Vf * (F / 60F) * (Tipo);
nhom.Text = AL.ToString();

// Do dai dich chuyen cua moi truc
double Ax = (AL / L) * (a - x);
double Ay = (AL / L) * (b - y);
double Az = (AL / L) * (c - z);
gocX.Text = Ax.ToString();
gocY.Text = Ay.ToString();
gocZ.Text = Az.ToString();
// So vong lap noi suy N
double T = L / (Vf * F / 60F);
double N1 = (T / Tipo);
int N = (int)N1;
// Gửi dữ liệu đi tính xung cho mỗi trục động cơ
tinhxung(Ax, Ay, Az, N);
xacdinhmag = 1;
}
private void tam_Click_1(object sender, EventArgs e)
{
}
// Ham dừng máy
private void STOP_Click(object sender, EventArgs e)
{
if (LbStatus.Text == "Connect") { Com.Write("s"); }
else
MessageBox.Show("Bạn chưa kết nối cổng Com...!");
}
private void test_Click(object sender, EventArgs e)
{
if (LbStatus.Text == "Connect") { Com.Write("o"); }
else

MessageBox.Show("Bạn chưa kết nối cổng Com...!");
}
String[] CT = new String[100000];
Int32 tangdem = 0;
/*//////////////////////////////////////G02 viết theo bán kính/////////////////////////////////////
private void G02()
{
xacdinhmag = 2;
float X0 = float.Parse(TTA1.Text);
float Y0 = float.Parse(TTA2.Text);
float Xf = float.Parse(CTA1.Text);
float Yf = float.Parse(CTA2.Text);
double R = double.Parse(CTB2.Text);
float blu = float.Parse(minstepx.Text) * 4;
float Xj, Yj;
double R = Math.Sqrt(Math.Pow(X0 - Xj, 2) + Math.Pow(Y0 - Yj, 2));
X0 = X0 - Xj;
Y0 = Y0 - Yj;
Xf = Xf - Xj;


Yf = Yf - Yj;
float N;
double Xk, Yk, Dk, tang = blu;
int GOC = 0;
it = 1;
// xác định góc phần tư
if (Xf > X0)
{
if (Y0 > Yf) { GOC = 1; }

if (Y0 < Yf) { GOC = 2; }
}
if (Xf < X0)
{
if (Yf < Y0) { GOC = 4; }
if (Yf > Y0) { GOC = 3; }
}
////////////////////////////////////// Tính toan ///////////////////////////////////////////
N = (Math.Abs(Xf - X0) + Math.Abs(Yf - Y0));
Xk = X0;
Yk = Y0;
do
{
tangdem++;
Dk = ((Xk * Xk + Yk * Yk) - (R * R));
if (Dk <= 0)
{
if (GOC == 1)
{
Xk = Xk + blu;
CT[tangdem] = "A";
}
if (GOC == 2)
{
Yk = Yk + blu;
CT[tangdem] = "V";
}
if (GOC == 3)
{
Xk = Xk - blu;

CT[tangdem] = "a";
}
if (GOC == 4)
{
Yk = Yk - blu;
CT[tangdem] = "v";
}
}
else


{
if (GOC == 1)
{
Yk = Yk - blu;
CT[tangdem] = "v";
}
if (GOC == 2)
{
Xk = Xk + blu;
CT[tangdem] = "A";
}
if (GOC == 3)
{
Yk = Yk + blu;
CT[tangdem] = "V";
}
if (GOC == 4)
{
Xk = Xk - blu;

CT[tangdem] = "a";
}
}
tang = tang + blu;
} while (tang <= N);
vtx.Clear();
vtz.Clear();
vty.Clear();
ctxung.Clear();
ctxung.Text += "R=" + R;
vtx.Text += "Goc=" + GOC;
vty.Text += tangdem;
Com.Write("G");
}
*/
/// //////////////////// G02 viết theo tọa độ tâm//////////////////////////////////////////////////////
private void G02()
{
xacdinhmag = 2;
float X0 = float.Parse(TTA1.Text);
float Y0 = float.Parse(TTA2.Text);
float Xf = float.Parse(CTA1.Text);
float Yf = float.Parse(CTA2.Text);
float Xi = float.Parse(CTB1.Text);
float Yi = float.Parse(CTB2.Text);
float blu = float.Parse( minstepx.Text)*4;
float Xj, Yj;
Xj = X0 + Xi; //vty.Clear(); vty.Text += Xj;
Yj = Y0 + Yi; //vtz.Clear(); vtz.Text += Yj;

double R = Math.Sqrt(Math.Pow(X0 - Xj, 2) + Math.Pow(Y0 - Yj, 2));
X0 = X0 - Xj;
Y0 = Y0 - Yj;
Xf = Xf - Xj;
Yf = Yf - Yj;
float N;
double Xk, Yk, Dk,tang=blu;
int GOC=0;
it = 1;
// xác định góc phần tư
if (Xf > X0) { if (Y0>Yf) { GOC=1; }
if (Y0}
if (Xf < X0)
{
if (Yf < Y0) { GOC = 4; }
if (Yf > Y0) { GOC = 3; }
}
///////////////////////// Tính toan ///////////////////////////////////////////
N = (Math.Abs(Xf - X0) + Math.Abs(Yf - Y0));
Xk = X0;
Yk = Y0;
do
{
tangdem++;
Dk=((Xk*Xk+Yk*Yk)-(R*R));
if (Dk <= 0)
{
if(GOC == 1)

{ Xk = Xk + blu;
CT[tangdem]="A";
}
if (GOC == 2)
{
Yk = Yk + blu;
CT[tangdem] = "V";
}
if (GOC == 3)
{
Xk = Xk - blu;
CT[tangdem] = "a";
}
if (GOC == 4)
{
Yk = Yk - blu;
CT[tangdem] = "v";
}
}


else
{
if (GOC == 1)
{
Yk = Yk - blu;
CT[tangdem] = "v";
}
if (GOC == 2)
{

Xk = Xk + blu;
CT[tangdem] = "A";
}
if (GOC == 3)
{
Yk = Yk + blu;
CT[tangdem] = "V";
}
if (GOC == 4)
{
Xk = Xk - blu;
CT[tangdem] = "a";
}
}
tang = tang + blu;
} while(tang<=N);
vtx.Clear();
vtz.Clear();
vty.Clear();
ctxung.Clear();
ctxung.Text += "R=" + R;
vtx.Text += "Goc=" + GOC;
vty.Text += tangdem;
if (xdct == 0) { Com.Write("G"); }
}
// Nội suy cung tròn ngược chiều kim đồng hồ
private void G03()
{
xacdinhmag = 2;
float X0 = float.Parse(TTA1.Text);

float Y0 = float.Parse(TTA2.Text);
float Xf = float.Parse(CTA1.Text);
float Yf = float.Parse(CTA2.Text);
float Xi = float.Parse(CTB1.Text);
float Yi = float.Parse(CTB2.Text);
float blu = float.Parse( minstepx.Text)*4;
float Xj, Yj;
Xj = X0 + Xi; vty.Clear(); vty.Text += Xj;
Yj = Y0 + Yi; vtz.Clear(); vtz.Text += Yj;