Code VB

Private Declare Function BeepAPI Lib "kernel32" Alias "Beep" (ByVal dwFrequency _
As Long, ByVal dwMilliseconds As Long) As Long
Private Declare Function CopyFile Lib "kernel32" Alias "CopyFileA" (ByVal
lpExistingFileName As String, ByVal lpNewFileName As String, ByVal bFailIfExists As
Long) As Long
Private Declare Function DeleteFile Lib "kernel32.dll" Alias "DeleteFileA" (ByVal
lpFileName As String) As Long
Private Declare Sub Sleep Lib "kernel32" (ByVal dwMilliseconds As Long) ' Tao ham
delay
'Su dung keyboard
Dim in_out_side, speed_change As Boolean
Dim atam_current(5), time As Single
'khai bao cho noi suy duong tron
Dim dem_slave As Byte
Dim time_line As Single
Dim conoisuy As Boolean
Dim x1_cir, y1_cir, z1_cir, x2_cir, y2_cir, z2_cir, x3_cir, y3_cir, z3_cir As Single
Dim a1x, a1y, a1z, a3x, a3y, a3z As Single
Dim center_x, center_y, center_z As Single
Dim cos12, cos13, cos23, cosx11, cosx12, cosx21, cosx22, cosy_11, cosy_12, cosy_22,
cos_tim As Single
Dim ax_cir12, ax_cir13, ax_cir23 As Single
Dim ay_cir12, ay_cir13, ay_cir23 As Single
Dim az_cir12, az_cir13, az_cir23 As Single
Dim A1_cir, B1_cir, C1_cir, D1_cir, A2_cir, B2_cir, C2_cir, D2_cir, A3_cir, B3_cir,
C3_cir, D3_cir, A4_cir, B4_cir, C4_cir, D4_cir, A5_cir, B5_cir, C5_cir, D5_cir, R2_cir
As Single
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Dim mistake As String
Dim check_data_save As Boolean
Dim mistake_keyboard As Boolean

Dim pi As Single
Dim speed_current(10000, 5) As Double
Dim speed_current_byte(10000, 5, 5) As Byte
Dim xung(10000, 6) As Single
Dim X_auto(10000), Y_auto(10000), z_auto(10000) As Single
Dim a(10000, 7) As Integer
Dim lancapnhat As Byte
Public khop1_home, khop2_home, khop4_home, h_home, khop1_hard, khop2_hard,
khop4_hard, h_hard, khop1_soft, khop2_soft, khop4_soft, h_soft As Single
Dim x_current(10000), y_current(10000), z_current(10000) As Single
Dim x_msf(10000), y_msf(10000), z_msf(10000) As Single

1


Dim a_current(5), v_current(5), p_current(10000, 5), v_tam(10000, 5) As Single
Dim max_period_line, max_point_tam As Single
Dim change_speed As Boolean
Dim goc1_current, goc2_current, goc4_current, h_current As Single
Dim a_no(5), speed_no, a_line(5), speed_line, a_cir(5), speed_cir As Single
Dim p_auto(10000, 5, 5), v_auto(10000, 5, 5), a_auto(10000, 5, 5) As Single
Dim point_number As Integer
Dim noisuy(10000) As Boolean
Dim tool(10000) As String
Dim end_point(10000) As Integer
Dim max_point As Integer
Dim error As Boolean
Dim second_time As Boolean
Dim khop2a, khop2b, khop1 As Single
'////////////////Khai bao cho fan thu thap du lieu

Dim count_sum, count_mu As Byte
Dim sum1, sum2, sum3, sum4, dem As Single
Dim ready_receive As Boolean
'//////////////////
Public timeanddate As String
Dim openwindown As Boolean
'''''Dim i As Integer, tabindex As Integer, k As Integer
Dim col As Byte
Dim row As Byte
Public countrow, countcol As Byte
Public maxrow, maxcol As Byte
Dim choose As Boolean
Dim dotposition As Byte
Public enable_drawpoint As Boolean
Dim harware_ready As Boolean
Public saved, savechange As Boolean
Dim robotrun, robotstop, robothome, robotautorun As Boolean
Dim b1_out, b2_out, b3_out, b4_out, b5_out, b6_out, b7_out, b8_out As Boolean
Dim tool_string, inter_string, speed_string As String
Dim timerun As Byte
Dim timeauto, timeauto_speed As Single
Dim speed As Byte
Dim D1, D2, D3, D4 As Single
Dim X, Y, z As Single
Dim mangx(1000) As Single
Dim mangy(1000) As Single
Dim mangz(1000) As Single
Dim mangkhop1rotate(1000) As Single
Dim mangkhop2rotate(1000) As Single

2


Dim mangkhop4rotate(1000) As Single
Dim manghrotate(1000) As Single
Dim khop1find, khop2find, khop3find, zfind As Single
Dim khau1x(1000) As Integer
Dim khau1y(1000) As Integer
Dim khau2x(1000) As Integer
Dim khau2y(1000) As Integer
Dim khau3x(1000) As Integer
Dim khau3y(1000) As Integer
Dim R1, G1, B1 As Byte
Dim R2, G2, B2 As Byte
Dim R3, G3, B3 As Byte
Dim R_point(1000) As Byte
'''''''''''''''''Khai bao cho do hoa
Dim ctrl, shift_button As Boolean
Dim change_tool As Boolean
Dim Button As String
Dim slowquick As Single
Dim scale_gra As Single
Dim M5C As Boolean
Dim obit, high, khop1rotate, khop2rotate, khop3rotate, khop4rotate As Single
Dim tamx, tamy, lenghx, lenghy, lenghz As Single
Public lenghelip1, widthelip, heightelip, lenghelip2 As Single
Public radiantru1a, heighttru1a As Single
Public radiantru1b, heighttru1b As Single
Public radiantru2, heighttru2 As Single
Public lenghkhoihop1, widthkhoihop1, heightkhoihop1 As Single

Public lenghkhoihop2, widthkhoihop2, heightkhoihop2 As Single
Dim phi As Single
Dim gocx, gocy, gocz As Integer
Dim rotateupdown As Single
Const MAXVERTS = 120
Const MAXEDGES = 120
Private Type POINT3D
X As Single
Y As Single
z As Single
End Type
Private Type POINT2D
X As Single

3


Y As Single
End Type
Private Type wireframe
NumVerts As Byte
NumEdges As Byte
Edge(MAXEDGES, 2) As Byte
Vert(MAXVERTS) As POINT3D
Verta(MAXVERTS) As POINT3D
VertTF(MAXVERTS) As POINT3D
VertTB(MAXVERTS) As POINT3D
Vertp(MAXVERTS) As POINT2D
End Type
Dim orgx As Integer, orgy As Integer

Private Sub Check1_Click() ' cho fep ket noi voi robot
If Check1.Value = 1 Then
Timer4.Enabled = False: harware_ready = False: Sleep (100)
Sta.Panels(1).Text = "Not ready ": Sta.Panels(2).Text = "Master or Comport has a
problem "
com.Output = "001"
Else
Timer4.Enabled = False: Sleep (100): Timer1.Interval = 200: Timer1.Enabled = True
khop1_home = khop1_soft: khop2_home = khop2_soft: khop4_home = khop4_soft:
h_home = h_soft
Sta.Panels(2).Text = "Unconnected to your robot."
If Check2.Value = 1 Then Check2.Value = 0: com.Output = "044"
End If
End Sub
Private Sub Check2_Click()
If Check2.Value = 1 Then
If Check1.Value = 1 And robotautorun = False And robothome = False Then
If robotrun = True Then
mnuautorun.Enabled = False: Command16.Enabled = False
Timer4.Enabled = False: Timer1.Enabled = False: Command12.Caption =
"Run": Sleep (100)
com.Output = "043"
ready_receive = False
Timer1.Interval = 1000: Timer1.Enabled = True
Sta.Panels(2).Text = "Connected to your keyboard."
Else
Check2.Value = 0
End If
Else

4


Check2.Value = 0
Timer4.Enabled = False: Sleep (100): Timer1.Interval = 200: Timer1.Enabled =
True
End If
Else
Timer4.Enabled = False: Sleep (100)
com.Output = "044"
Timer4.Enabled = True: Timer1.Enabled = False
Sta.Panels(2).Text = "Unconnected to your keyboard."
End If
End Sub
Private Sub com_OnComm()
If com.CommEvent = comEvReceive Then
data = com.Input
Label15.Caption = Asc(data)
If speed_change = True Then
mnuspeednumber_Click (Asc(data) - 3)
BeepAPI 2000, 50
speed_change = False
End If
If ready_receive = True Then
If count_sum < 4 Then sum1 = sum1 + Asc(data) * (256 ^ count_mu)
If count_sum > 3 And count_sum < 8 Then sum2 = sum2 + Asc(data) * (256 ^
count_mu)
If count_sum > 7 And count_sum < 12 Then sum3 = sum3 + Asc(data) * (256 ^
count_mu)
If count_sum > 11 And count_sum < 16 Then sum4 = sum4 + Asc(data) * (256 ^

count_mu)
count_sum = count_sum + 1: count_mu = count_mu + 1
If count_mu = 4 Then count_mu = 0
If count_sum = 16 Then
count_sum = 0
lancapnhat = lancapnhat + 1
ready_receive = False
'//////////////////////////////////////////////////////////////////////////////
If sum1 > 65536 Then sum1 = Right(CStr(sum1), 5) - 67295
If sum2 > 65536 Then sum2 = Right(CStr(sum2), 5) - 67295
If sum3 > 65536 Then sum3 = Right(CStr(sum3), 5) - 67295
If sum4 > 65536 Then sum4 = Right(CStr(sum4), 5) - 67295
Label23.Caption = sum1: Label28.Caption = sum2
Label29.Caption = sum3: Label32.Caption = sum4
'tinh toan ra thong so cu the de giam sat/////////////////////////////

5


khop1rotate = (360 * sum1) / (4 * optionform.ppr1 * optionform.ijoint1)
khop2rotate = (360 * sum2) / (4 * optionform.ppr2 * optionform.ijoint2)
khop3rotate = (360 * sum3) / (4 * optionform.ppr3 * optionform.ijoint3)
khop4rotate = (360 * sum4) / (4 * optionform.ppr4 * optionform.ijoint4)
high = Round((khop3rotate / 360) * 20, 3)
'/////////////////////////////////////////////////////////////////////
X = (lenghelip2 * Cos((khop1rotate + khop2rotate) * pi / 180) + lenghelip1 *
Cos(khop1rotate * pi / 180)) / scale_gra
Y = (lenghelip2 * Sin((khop1rotate + khop2rotate) * pi / 180) + lenghelip1 *
Sin(khop1rotate * pi / 180)) / scale_gra
z = (optionform.d1_lengh_tam / 2.5 - high * scale_gra / 2.5 - 100 * scale_gra) /

scale_gra
Text(9).Text = CStr(Round(X * 2.5, 3))
Text(10).Text = CStr(Round(Y * 2.5, 3))
Text(11).Text = CStr(Round(z * 2.5, 3))
xung(0, 1) = sum1: xung(0, 2) = sum2: xung(0, 3) = sum3: xung(0, 4) = sum4
Text(5).Text = CStr(Round(khop1rotate, 3))
Text(6).Text = CStr(Round(khop2rotate, 3))
Text(7).Text = CStr(Round(khop4rotate, 3))
If robothome = True Then
If ProgressBar1.Value < ProgressBar1.Max - 3 Then ProgressBar1.Value =
ProgressBar1.Value + 3
If Abs(CSng(Text(5).Text) - khop1_home) < 5 And Abs(CSng(Text(6).Text) khop2_home) < 5 And Abs(high - h_home) < 5 And Abs(CSng(Text(7).Text) khop4_home) < 5 Then
ProgressBar1.Value = ProgressBar1.Max: Command11.Caption = "Home":
mnuhome.Enabled = True: robothome = False: Sta.Panels(2).Text = "Robot is at home"
End If
End If
If robotautorun = True Then
If ProgressBar1.Value < maxrow Then MSFdata.row = ProgressBar1.Value +
1: BeepAPI 5000, 100
If tool(ProgressBar1.Value + 1) = "Open" Or tool(ProgressBar1.Value + 1) = "
Open" Then
M5C = False
Else
M5C = True
End If
MSFdata.col = 7
If Abs(CSng(Text(5).Text) - CSng(MSFdata.Text)) < 7 Then
MSFdata.col = 8
If Abs(CSng(Text(6).Text) - CSng(MSFdata.Text)) < 7 Then

6


MSFdata.col = 3
If Abs(CSng(Text(11).Text) - CSng(MSFdata.Text)) < 7 Then
MSFdata.col = 10
If Abs(CSng(Text(7).Text) - CSng(MSFdata.Text)) < 7 Then
ProgressBar1.Value = ProgressBar1.Value + 1: Sta.Panels(2).Text = "Finished point " +
CStr(ProgressBar1.Value)
End If
If robotautorun = True And ProgressBar1.Value = maxrow Then
Command16.Caption = "Auto": Command16.Enabled = False: mnuautorun.Enabled =
False: robotautorun = False: Check1.Enabled = True: Check2.Enabled = True:
Sta.Panels(2).Text = "Robot has finished autorun"
End If
End If
End If
'///////////////////////////////////Xet xem co ngoai vung hoat dong khong
If lancapnhat > 1 Then
lancapnhat = 1
check_in_out
If in_out_side = True Then
If Check2.Value = 1 Then Button = "": stop_robot
in_out_side = False: GoTo Stop_outside
End If
GoTo Continue
Stop_outside:
com.RTSEnable = False: com.RTSEnable = True
Command1.BackColor = &H8000000F: Command2.BackColor =
&H8000000F: Command3.BackColor = &H8000000F: Command7.BackColor =

&H8000000F
Command4.BackColor = &H8000000F: Command5.BackColor =
&H8000000F: Command6.BackColor = &H8000000F: Command8.BackColor =
&H8000000F
Button = ""
BeepAPI 2000, 600
ex = MsgBox("Out of the operation side, go back please !!! ", vbYes, "
Caution")
Continue:
Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3
If enable_drawpoint = True Then
For t = 1 To max_point
DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t
Next t
End If
End If
sum1 = 0: sum2 = 0: sum3 = 0: sum4 = 0
End If

7


End If
Dim checkcom As String
checkcom = data
checkcom = Left(checkcom, 8)
Select Case checkcom
Case "comready":
harware_ready = True
Timer4.Enabled = True: Timer1.Enabled = False

Command16.Enabled = False: mnuautorun.Enabled = False
khop1_home = khop1_hard: khop2_home = khop2_hard: khop4_home =
khop4_hard: h_home = h_hard
Command14.Caption = "&Stop": Command16.Caption = "&Auto":
Command11.Caption = "&Home": Command12.Caption = "&Run"
Sta.Panels(1).Text = "Ready ": Sta.Panels(2).Text = "Your system has
connected well"
Case "slave1no": Sta.Panels(1).Text = "Not ready ": Sta.Panels(2).Text = "Your
hardware has a problem (slave 1). "
Case "slave2no": Sta.Panels(1).Text = "Not ready ": Sta.Panels(2).Text = "Your
hardware has a problem (slave 2). "
Case "slave3no": Sta.Panels(1).Text = "Not ready ": Sta.Panels(2).Text = "Your
hardware has a problem (slave 3). "
Case "slave4no": Sta.Panels(1).Text = "Not ready ": Sta.Panels(2).Text = "Your
hardware has a problem (slave 4). "
Case "line_int": BeepAPI 2000, 50: Sleep (50): mnuinterpolate(1).Checked = False:
mnuinterpolate(3).Checked = False: mnuinterpolate(2).Checked = True: inter_string =
"Line": Sta.Panels(2).Text = "You have just chosen line interpolate"
Case "circ_int": BeepAPI 2000, 50: Sleep (50): mnuinterpolate(1).Checked = False:
mnuinterpolate(2).Checked = False: mnuinterpolate(3).Checked = True: inter_string =
"Circle": Sta.Panels(2).Text = "You have just chosen circle interpolate"
Case "saveposi": mnuinterpolate(1).Checked = True: mnuinterpolate(2).Checked =
False: mnuinterpolate(3).Checked = False: save_data: inter_string = "No"
Case "auto_key": BeepAPI 2000, 50: Sleep (50): mistake_keyboard = False:
compile_data: If mistake_keyboard = False Then MousePointer = 13: autorun_robot:
robotautorun = True
Case "auto_fin": Command16.Caption = "Auto": robotautorun = False:
Check1.Enabled = True: Check2.Enabled = True: Sta.Panels(2).Text = "Robot has
finished auto running": ProgressBar1.Value = ProgressBar1.Max
Case "stop_key": Command16.Caption = "Auto": robotautorun = False:

Check1.Enabled = True: Check2.Enabled = True: Sta.Panels(2).Text = "Robot has
stopped."
Case "tool_ope": tool_string = "Open": M5C = False: Sta.Panels(2).Text = "Tool
openned."
Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3

8


If enable_drawpoint = True Then
For t = 1 To max_point
DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t
Next t
End If
Case "tool_clo": tool_string = "Close": M5C = True: Sta.Panels(2).Text = "Tool
closed."
Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3
If enable_drawpoint = True Then
For t = 1 To max_point
DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t
Next t
End If
Case "get_data": Timer1.Enabled = False: lancapnhat = 1: ready_receive = True:
Command12.Caption = "Run"
Case "speed000": speed_change = True
Case "command1": Button = "command1": Sta.Panels(2).Text = "Joint 1 is rotating
right."
Case "command2": Button = "command2": Sta.Panels(2).Text = "Joint 1 is rotating
left."
Case "command3": Button = "command3": Sta.Panels(2).Text = "Joint 2 is rotating

right."
Case "command4": Button = "command4": Sta.Panels(2).Text = "Joint 2 is rotating
left."
Case "command5": Button = "command5": Sta.Panels(2).Text = "Tool is rotating
right."
Case "command6": Button = "command6": Sta.Panels(2).Text = "Tool is rotating
left."
Case "command7": Button = "command7": Sta.Panels(2).Text = "Tool is moving
up."
Case "command8": Button = "command8": Sta.Panels(2).Text = "Tool is moving
down."
End Select
outside:
End If
End Sub
Private Sub check_in_out()
If CSng(Text(5).Text) > CSng(optionform.k1_max) And Button = "command1"
Then b1_out = True: in_out_side = True: GoTo Stop_outside
If CSng(Text(5).Text) < CSng(optionform.k1_min) And Button = "command2"
Then b2_out = True: in_out_side = True: GoTo Stop_outside
If CSng(Text(6).Text) > CSng(optionform.k2_max) And Button = "command3"
Then b3_out = True: in_out_side = True: GoTo Stop_outside

9


If CSng(Text(6).Text) < CSng(optionform.k2_min) And Button = "command4"
Then b4_out = True: in_out_side = True: GoTo Stop_outside
If CSng(Text(11).Text) > CSng(optionform.k3_max) And Button =
"command7" Then b7_out = True: in_out_side = True: GoTo Stop_outside

If CSng(Text(11).Text) < CSng(optionform.k3_min) And Button = "command8"
Then b8_out = True: in_out_side = True: GoTo Stop_outside
If CSng(Text(7).Text) > CSng(optionform.k4_max) And Button = "command6"
Then b6_out = True: in_out_side = True: GoTo Stop_outside
If CSng(Text(7).Text) < CSng(optionform.k4_min) And Button = "command5"
Then b5_out = True: in_out_side = True: GoTo Stop_outside
Stop_outside:
End Sub
Private Sub Command1_mousedown(Button1 As Integer, Shift As Integer, X As Single,
Y As Single) 'xoay fai khop thu 1
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command1.BackColor = &HFFC0C0
R1 = 255: G2 = 255: B3 = 255: G1 = 0: B1 = 0: R2 = 0: B2 = 0: R3 = 0: G3 = 0
Button = "command1"
If Check1.Value = 1 Then
' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b1_out = False Then
Timer4.Enabled = False: Sleep (100)
b2_out = False
com.Output = "020"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
Else
Timer10.Enabled = True
End If

mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Joint 1 is rotating right."
End If
thoat:
End Sub

10


Private Sub Command1_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 1
If robotrun = True And Check2.Value = 0 Then
Command1.BackColor = &H8000000F
Timer10.Enabled = False
Button = ""
Sta.Panels(2).Text = "Joint 1 stop rotating right."
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Command13_Click()
If Label9.Visible = True Then
Label9.Visible = False
Else
Label9.Visible = True
End If
End Sub
Private Sub Command2_mousedown(Button1 As Integer, Shift As Integer, X As Single,

Y As Single) 'ngung xoay fai khop thu 1
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command2.BackColor = &HFFC0C0
R1 = 255: G2 = 255: B3 = 255: G1 = 0: B1 = 0: R2 = 0: B2 = 0: R3 = 0: G3 = 0
Button = "command2"
If Check1.Value = 1 Then
'
If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b2_out = False Then
Timer4.Enabled = False: Sleep (100)
b1_out = False
com.Output = "021"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
Else
Timer10.Enabled = True
End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Joint 1 is rotating left."

11


End If
thoat:

End Sub
Private Sub Command2_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 1
If robotrun = True And Check2.Value = 0 Then
Command2.BackColor = &H8000000F
Timer10.Enabled = False
Button = ""
Sta.Panels(2).Text = "Joint 1 stop rotating left."
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Auto_practical()
Sta.Panels(2).Text = "Transferring data, please wait.": Sleep (1000)
ProgressBar1.Value = 0: ProgressBar1.Max = max_point
com.Output = "003": Sleep (2)
For i = 1 To max_point
ProgressBar1.Value = i: Sta.Panels(2).Text = "Point " + CStr(i) + "/" +
CStr(max_point) + " is transferred."
For j = 1 To 4
For dem_byte = 1 To 4
com.Output = Left("000", 3 - Len(CStr(p_auto(i, j, dem_byte)))) +
CStr(p_auto(i, j, dem_byte)): Sleep (2)
Next dem_byte
For dem_byte = 1 To 4
com.Output = Left("000", 3 - Len(CStr(v_auto(i, j, dem_byte)))) +
CStr(v_auto(i, j, dem_byte)): Sleep (2)
Next dem_byte

For dem_byte = 1 To 4
com.Output = Left("000", 3 - Len(CStr(a_auto(i, j, dem_byte)))) +
CStr(a_auto(i, j, dem_byte)): Sleep (2)
Next dem_byte
Next j
If tool(i) = "Open" Then com.Output = "000"
If tool(i) = "Close" Then com.Output = "001"
Sleep (1)
Next i
com.Output = "999": Sleep (2)
ProgressBar1.Value = 0

12


If Check2.Value = 0 And Check1.Value = 1 Then Timer4.Enabled = True: lancapnhat =
0: Timer1.Enabled = False
If Check1.Value = 0 Then Timer1.Interval = 300: Timer1.Enabled = True
End Sub
Private Sub goc(ByVal x1_goc As Single, ByVal y1_goc As Single, ByVal z1_goc As
Single, ByVal x2_goc As Single, ByVal y2_goc As Single, ByVal z2_goc As Single,
ByVal tam_x As Single, ByVal tam_y As Single, ByVal tam_z As Single, ByVal dem As
String)
Dim chiphuong1(4), chiphuong2(4), cos_goc, costam As Single
chiphuong1(1) = x1_goc - tam_x: chiphuong1(2) = y1_goc - tam_y: chiphuong1(3) =
z1_goc - tam_z
chiphuong2(1) = x2_goc - tam_x: chiphuong2(2) = y2_goc - tam_y: chiphuong2(3) =
z2_goc - tam_z
costam = (chiphuong1(1) * chiphuong2(1) + chiphuong1(2) * chiphuong2(2) +
chiphuong1(3) * chiphuong2(3)) / (Sqr(chiphuong1(1) ^ 2 + chiphuong1(2) ^ 2 +

chiphuong1(3) ^ 2) * Sqr(chiphuong2(1) ^ 2 + chiphuong2(2) ^ 2 + chiphuong2(3) ^ 2))
Select Case dem
Case "cos12": cos12 = costam
Case "cos13": cos13 = costam
Case "cos23": cos23 = costam
Case "cosx11": cosx11 = costam
Case "cosx12": cosx12 = costam
Case "cosx21": cosx21 = costam
Case "cosx22": cosx22 = costam
Case "cosy_11": cosy_11 = costam
Case "cosy_12": cosy_12 = costam
Case "cosy_21": cosy_21 = costam
Case "cosy_22": cosy_22 = costam
Case "cos_tim": cos_tim = costam
End Select
End Sub
Private Sub tam(ByVal x1_cir As Single, ByVal y1_goc As Single, ByVal z1_goc As
Single, ByVal x2_goc As Single, ByVal y2_goc As Single, ByVal z2_goc As Single,
ByVal tam_x As Single, ByVal tam_y As Single, ByVal tam_z As Single)
Dim mid12_x, mid12_y, mid12_z, mid23_x, mid23_y, mid23_z As Single
Dim distance12, distance23, distance13 As Single
Dim U1_cir, V1_cir, W1_cir, U2_cir, V2_cir, W2_cir, U3_cir, V3_cir, W3_cir As Single
Dim tu_z, mau_z, tu_y, mau_y, tu_x, mau_x As Single
ax_cir12 = -(x1_cir - x2_cir): ax_cir13 = -(x1_cir - x3_cir): ax_cir23 = -(x2_cir - x3_cir)
ay_cir12 = -(y1_cir - y2_cir): ay_cir13 = -(y1_cir - y3_cir): ay_cir23 = -(y2_cir - y3_cir)
az_cir12 = -(z1_cir - z2_cir): az_cir13 = -(z1_cir - z3_cir): az_cir23 = -(z2_cir - z3_cir)

13

distance12 = Sqr(ax_cir12 ^ 2 + ay_cir12 ^ 2 + az_cir12 ^ 2)
distance13 = Sqr(ax_cir13 ^ 2 + ay_cir13 ^ 2 + az_cir13 ^ 2)
distance23 = Sqr(ax_cir23 ^ 2 + ay_cir23 ^ 2 + az_cir23 ^ 2)
If distance13 < distance12 Or distance13 < distance23 Then mistake = "over_cir": GoTo
thoat
mid12_x = (x1_cir + x2_cir) / 2: mid12_y = (y1_cir + y2_cir) / 2: mid12_z = (z1_cir +
z2_cir) / 2
mid23_x = (x2_cir + x3_cir) / 2: mid23_y = (y2_cir + y3_cir) / 2: mid23_z = (z2_cir +
z3_cir) / 2
A1_cir = ax_cir12: B1_cir = ay_cir12: C1_cir = az_cir12: D1_cir = A1_cir * mid12_x +
B1_cir * mid12_y + C1_cir * mid12_z
A2_cir = ax_cir23: B2_cir = ay_cir23: C2_cir = az_cir23: D2_cir = A2_cir * mid23_x +
B2_cir * mid23_y + C2_cir * mid23_z
A3_cir = ay_cir12 * az_cir13 - ay_cir13 * az_cir12
B3_cir = az_cir12 * ax_cir13 - ax_cir12 * az_cir13
C3_cir = ax_cir12 * ay_cir13 - ay_cir12 * ax_cir13
D3_cir = A3_cir * x1_cir + B3_cir * y1_cir + C3_cir * z1_cir
tu_z = (D3_cir - D1_cir * A3_cir / A1_cir) - (D2_cir - D1_cir * A2_cir / A1_cir) *
((B3_cir - B1_cir * A3_cir / A1_cir) / (B2_cir - B1_cir * A2_cir / A1_cir))
mau_z = (C3_cir - C1_cir * A3_cir / A1_cir) - (C2_cir - C1_cir * A2_cir / A1_cir) *
((B3_cir - B1_cir * A3_cir / A1_cir) / (B2_cir - B1_cir * A2_cir / A1_cir))
center_z = tu_z / mau_z
tu_y = (D2_cir - D1_cir * A2_cir / A1_cir) - (C2_cir - C1_cir * A2_cir / A1_cir) *
center_z
mau_y = B2_cir - B1_cir * A2_cir / A1_cir
center_y = tu_y / mau_y
tu_x = D1_cir - C1_cir * center_z - B1_cir * center_y
mau_x = A1_cir
center_x = tu_x / mau_x
thoat:

End Sub
Private Sub Command3_mousedown(Button1 As Integer, Shift As Integer, X As Single,
Y As Single) 'xoay fai khop thu 2
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else

14


Command3.BackColor = &HFFC0C0
G1 = 255: R2 = 255: B3 = 255: R1 = 0: B1 = 0: G2 = 0: B2 = 0: R3 = 0: G3 = 0
Button = "command3"
If Check1.Value = 1 Then
' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b3_out = False Then
Timer4.Enabled = False: Sleep (100)
b4_out = False
com.Output = "023"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
Else
Timer10.Enabled = True
End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Joint 2 is rotating right."
End If

thoat:
End Sub
Private Sub Command3_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 2
If robotrun = True And Check2.Value = 0 Then
Command3.BackColor = &H8000000F
Button = ""
Timer10.Enabled = False
Sta.Panels(2).Text = "Joint 2 stop rotating right."
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Command4_mousedown(Button1 As Integer, Shift As Integer, X As Single,
Y As Single) 'xoay fai khop thu 2
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command4.BackColor = &HFFC0C0
G1 = 255: R2 = 255: B3 = 255: R1 = 0: B1 = 0: G2 = 0: B2 = 0: R3 = 0: G3 = 0
Button = "command4"
If Check1.Value = 1 Then

15

' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b4_out = False Then
Timer4.Enabled = False: Sleep (100)
b3_out = False
com.Output = "024"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
Else
Timer10.Enabled = True
End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Joint 2 is rotating left."
End If
thoat:
End Sub
Private Sub Command4_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 2
If robotrun = True And Check2.Value = 0 Then
Command4.BackColor = &H8000000F
Button = ""
Timer10.Enabled = False
Sta.Panels(2).Text = "Joint 2 stop rotating left"
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Command5_mousedown(Button1 As Integer, Shift As Integer, X As Single,

Y As Single) 'xoay fai khop thu 2
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command5.BackColor = &HFFC0C0
R3 = 255: G1 = 255: G2 = 255: R1 = 0: B1 = 0: R2 = 0: B2 = 0: G3 = 0: B3 = 0
Button = "command5"
If Check1.Value = 1 Then
'
If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b5_out = False Then

16


Timer4.Enabled = False: Sleep (100)
b6_out = False
com.Output = "030"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
Else
Timer10.Enabled = True
End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Tool is rotating left."
End If
thoat:

End Sub
Private Sub Command5_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 2
If robotrun = True And Check2.Value = 0 Then
Command5.BackColor = &H8000000F
Button = ""
Sta.Panels(2).Text = "Tool stop rotating left."
Timer10.Enabled = False
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Command6_mousedown(Button1 As Integer, Shift As Integer, X As Single,
Y As Single) 'xoay fai khop thu 2
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command6.BackColor = &HFFC0C0
R3 = 255: G1 = 255: G2 = 255: R1 = 0: B1 = 0: R2 = 0: B2 = 0: G3 = 0: B3 = 0
Button = "command6"
If Check1.Value = 1 Then
' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b6_out = False Then
Timer4.Enabled = False: Sleep (100)
b5_out = False

com.Output = "029"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'

17


End If
Else
Timer10.Enabled = True
End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Tool is rotating right."
End If
thoat:
End Sub
Private Sub Command6_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 2
If robotrun = True And Check2.Value = 0 Then
Command6.BackColor = &H8000000F
Button = ""
Sta.Panels(2).Text = "Tool stop rotating right"
Timer10.Enabled = False
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Command7_mousedown(Button1 As Integer, Shift As Integer, X As Single,
Y As Single) 'xoay fai khop thu 2

If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command7.BackColor = &HFFC0C0
R3 = 255: G1 = 255: G2 = 255: R1 = 0: B1 = 0: R2 = 0: B2 = 0: G3 = 0: B3 = 0
Button = "command7"
If Check1.Value = 1 Then
' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b7_out = False Then
Timer4.Enabled = False: Sleep (100)
b8_out = False
com.Output = "027"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
Else
Timer10.Enabled = True

18


End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Tool is moving up."
End If
thoat:
End Sub
Private Sub Command7_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y

As Single) 'ngung xoay fai khop thu 2
If robotrun = True And Check2.Value = 0 Then
Command7.BackColor = &H8000000F
Button = ""
Sta.Panels(2).Text = "Tool stop moving up."
Timer10.Enabled = False
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Command8_mousedown(Button1 As Integer, Shift As Integer, X As Single,
Y As Single) 'xoay fai khop thu 2
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
Command8.BackColor = &HFFC0C0
R3 = 255: G1 = 255: G2 = 255: R1 = 0: B1 = 0: R2 = 0: B2 = 0: G3 = 0: B3 = 0
Button = "command8"
If Check1.Value = 1 Then
' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware has a
problem": GoTo thoat
If b8_out = False Then
Timer4.Enabled = False: Sleep (100)
b7_out = False
com.Output = "026"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'

End If
Else
Timer10.Enabled = True
End If
mnuautorun.Enabled = False: Command16.Enabled = False
Sta.Panels(2).Text = "Tool is moving down."
End If

19


thoat:
End Sub
Private Sub Command8_mouseup(Button1 As Integer, Shift As Integer, X As Single, Y
As Single) 'ngung xoay fai khop thu 2
If robotrun = True And Check2.Value = 0 Then
Command8.BackColor = &H8000000F
Button = ""
Sta.Panels(2).Text = "Tool stop moving down."
Timer10.Enabled = False
If Check1.Value = 1 Then
com.RTSEnable = False
com.RTSEnable = True
End If
End If
End Sub
Private Sub Checkdata_save() ' Luu toa do hien tai vao bang
If CSng(Text(5).Text) > CSng(optionform.k1_max) Then check_data_save = True
If CSng(Text(5).Text) < CSng(optionform.k1_min) Then check_data_save = True
If CSng(Text(6).Text) > CSng(optionform.k2_max) Then check_data_save = True

If CSng(Text(6).Text) < CSng(optionform.k2_min) Then check_data_save = True
If CSng(Text(11).Text) > CSng(optionform.k3_max) Then check_data_save = True
If CSng(Text(11).Text) < CSng(optionform.k3_min) Then check_data_save = True
If CSng(Text(7).Text) > CSng(optionform.k4_max) Then check_data_save = True
If CSng(Text(7).Text) < CSng(optionform.k4_min) Then check_data_save = True
End Sub
Private Sub Command10_Click() ' Luu toa do hien tai vao bang
Checkdata_save
If check_data_save = True Then ex = MsgBox("Out of the operation side, go back
please !!! ", vbYes, " Caution"): check_data_save = False: GoTo thoat
save_data
thoat:
End Sub
Private Sub save_data() 'Luu du lieu vao bang
If choose = False Then countrow = maxrow + 1
If choose = True Then countrow = row: choose = False
MSFdata.row = countrow
For countcol = 5 To 11
If Text(countcol).Text = "" Then
Text(countcol).Text = "0"
End If

20


Next countcol
For countcol = 1 To 3
MSFdata.col = countcol
dotposition = InStr(1, Text(countcol + 8).Text, ".")
If dotposition = 1 Then

Text(countcol + 8).Text = "0" + Text(countcol + 8).Text
End If
MSFdata = Text(countcol + 8).Text
Next countcol
MSFdata.col = 4: MSFdata = tool_string
MSFdata.col = 5: MSFdata = inter_string
MSFdata.col = 6: MSFdata = speed_string
For countcol = 7 To 8
MSFdata.col = countcol
MSFdata = Text(countcol - 2).Text
Next countcol
MSFdata.col = 9: MSFdata = CStr(Round((350 - CSng(Text(11).Text)) / 20, 2))
MSFdata.col = 10: MSFdata = Text(7).Text
If countrow > maxrow Then
maxrow = countrow
End If
savechange = False
Command16.Enabled = False
mnuautorun.Enabled = False
Sta.Panels(2).Text = "Data is saved at row " + CStr(countrow)
BeepAPI 1600, 100: BeepAPI 2000, 60
End Sub
Private Sub Command9_click() 'dong mo tay kep
If robotrun = False Then Sta.Panels(2).Text = "Your system isn't ready to run.": GoTo
thoat
If robothome = True Or robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
If M5C = False Then
M5C = True: tool_string = "Close"

Else
M5C = False: tool_string = "Open"

21


End If
If Check1.Value = 1 Then
' If harware_ready = False Then Sta.Panels(2).Text = "Your hardware
has a problem": GoTo thoat
Timer4.Enabled = False
Sleep (100)
If M5C = True Then tool_string = "Close": com.Output = "033":
Sta.Panels(2).Text = "Tool closed"
If M5C = False Then tool_string = "Open": com.Output = "032":
Sta.Panels(2).Text = "Tool opened"
Timer4.Enabled = True: lancapnhat = 0 'co ket noi voi fan cung'
End If
mnuautorun.Enabled = False: Command16.Enabled = False
If M5C = True Then Sta.Panels(2).Text = "Tool closed"
If M5C = False Then Sta.Panels(2).Text = "Tool opened"
End If
Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3
If enable_drawpoint = True Then
For t = 1 To max_point
DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t
Next t
End If
thoat:
End Sub

Private Sub Command12_Click() ' cho phep robot hoat dong
run_robot
End Sub
Private Sub mnurun_Click()
run_robot
End Sub
Private Sub run_robot()
If robotrun = False Then
Command14.Caption = "Stop"
Command16.Caption = "Auto"
Command11.Caption = "Home"
Command12.Caption = "Run"
mnustop.Enabled = True: mnuhome.Enabled = True
robotrun = True: robotstop = False
If timerun = 0 Then
khop1rotate = 0#: khop2rotate = 0#: khop3rotate = 0#: khop4rotate = 0#: high = 0
X = (lenghelip2 * Cos((khop1rotate + khop2rotate) * pi / 180) + lenghelip1 *
Cos(khop1rotate * pi / 180)) / scale_gra

22


Y = (lenghelip2 * Sin((khop1rotate + khop2rotate) * pi / 180) + lenghelip1 *
Sin(khop1rotate * pi / 180)) / scale_gra
z = (optionform.d1_lengh_tam / 2.5 - high * scale_gra / 2.5 - 100 * scale_gra) /
scale_gra
Text(5).Text = CStr(Round(khop1rotate, 2))
Text(6).Text = CStr(Round(khop2rotate, 2))
Text(7).Text = CStr(Round(khop4rotate, 2))
Text(9).Text = CStr(Round(X * 2.5, 2))

Text(10).Text = CStr(Round(-Y * 2.5, 2))
Text(11).Text = CStr(Round(z * 2.5, 2))
End If
timerun = timerun + 1
Drawrobot 0, 255, 0, 0, 255, 0, 0, 0, 255
If enable_drawpoint = True Then
For t = 1 To max_point
DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t
Next t
End If
Sta.Panels(2).Text = "Your system is ready to run"
End If
End Sub
Private Sub Command14_Click() ' dung robot (stop)
stop_robot
End Sub
Private Sub mnustop_Click()
stop_robot
End Sub
Private Sub stop_robot() ' dung robot (stop)
If Check1.Value = 1 Then
Timer4.Enabled = False: Sleep (100)
com.Output = "888": Sleep (3)
If Check2.Value = 1 Then Check2.Value = 0: Check2_Click
Timer4.Enabled = True
End If
robotautorun = False: robotrun = False: robotstop = True: robothome = False
Command12.Caption = "Run": Command14.Caption = "Stop": Command11.Caption =
"Home": Command16.Caption = "Auto": Command16.Enabled = False
mnurun.Enabled = True: mnuhome.Enabled = True

Button = ""
Timer7.Enabled = False
Timer9.Enabled = False
Check1.Enabled = True: Check2.Enabled = True
Sta.Panels(2).Text = "Robot stop running"

23


End Sub
Private Sub Command11_Click() ' quay ve home
home_robot
End Sub
Private Sub mnuhome_Click()
home_robot
End Sub
Private Sub home_robot() ' quay ve home
If robotrun = False Then
Sta.Panels(2).Text = "Your system isn't ready to run."
Else
If robotautorun = True Or Check2.Value = 1 Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else
ProgressBar1.Max = 100: ProgressBar1.Value = 0: lancapnhat = 0
Command12.Caption = "Run": Command14.Caption = "Stop":
Command11.Caption = "Home": Command16.Caption = "Auto"
robothome = True: mnuautorun.Enabled = False: Command16.Enabled = False
G1 = 255: G2 = 255: B3 = 255: R1 = 0: B1 = 0: R2 = 0: B2 = 0: R3 = 0: G3 = 0
If Check1.Value = 1 Then
Timer4.Enabled = False: Sleep (100)

com.Output = "039": Sleep (5)
Timer4.Enabled = True: lancapnhat = 0
Else
Timer7.Enabled = True
End If
Sta.Panels(2).Text = "Robot is going to home"
End If
End If
End Sub
Private Sub Command16_Click() 'Autorun
autorun_robot
End Sub
Private Sub mnuautorun_Click()
autorun_robot
End Sub
Private Sub autorun_robot() 'Autorun
If robotrun = False Then
Sta.Panels(2).Text = "Your system isn't ready to run."
Else
If robothome = True Then
Sta.Panels(2).Text = "System is busy so it can't excute your command."
Else

24


If Check1.Value = 1 And conoisuy = True Then ex = MsgBox("Robot can't
operation with interpolate mode !!! ", vbYes, " Caution"): GoTo thoat
ProgressBar1.Value = 0: ProgressBar1.Max = maxrow: lancapnhat = 0
Check1.Enabled = False: Check2.Enabled = False: robotautorun = True

Command12.Caption = "Run": Command14.Caption = "Stop":
Command11.Caption = "Home": Command16.Caption = "Auto"
Sta.Panels(2).Text = "System is running automatically."
If Check1.Value = 1 Then
MousePointer = 13
Timer4.Enabled = False: Timer1.Enabled = False: Sleep (120)
Auto_practical
MousePointer = 0
Else
ProgressBar1.Value = 0: ProgressBar1.Max = 100: timeauto = 1: timeauto_speed
=1
Auto_illustrator
End If
End If
End If
thoat:
End Sub
Private Sub Label9_Click()
Label9.Caption = ""
End Sub
Private Sub mnuabow_Click() ' them 1 dong ben tren dong da chon
add_abow
End Sub
Private Sub mnu_abow_Click()
add_abow
End Sub
Private Sub add_abow()
If robothome = True Or robotautorun = True Then
Sta.Panels(2).Text = "Can't do this command because system is busy."
Else

If choose = True And row <= maxrow Then
choose = False
For countrow = maxrow + 1 To row Step -1
For countcol = 1 To 10
MSFdata.row = countrow
MSFdata.col = countcol
temperature = MSFdata.Text
MSFdata.row = countrow + 1
MSFdata.Text = temperature
Next countcol

25