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 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 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 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 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 = 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 = Then Check2.Value = 0: com.Output = "044" End If End Sub Private Sub Check2_Click() If Check2.Value = Then If Check1.Value = 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 = End If Else Check2.Value = 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 < Then sum1 = sum1 + Asc(data) * (256 ^ count_mu) If count_sum > And count_sum < Then sum2 = sum2 + Asc(data) * (256 ^ count_mu) If count_sum > 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 + If count_mu = Then count_mu = If count_sum = 16 Then count_sum = lancapnhat = lancapnhat + 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 thong so cu the de giam sat///////////////////////////// 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 - Then ProgressBar1.Value = ProgressBar1.Value + If Abs(CSng(Text(5).Text) - khop1_home) < And Abs(CSng(Text(6).Text) khop2_home) < And Abs(high - h_home) < And Abs(CSng(Text(7).Text) khop4_home) < 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 = If Abs(CSng(Text(5).Text) - CSng(MSFdata.Text)) < Then MSFdata.col = If Abs(CSng(Text(6).Text) - CSng(MSFdata.Text)) < Then MSFdata.col = If Abs(CSng(Text(11).Text) - CSng(MSFdata.Text)) < Then MSFdata.col = 10 If Abs(CSng(Text(7).Text) - CSng(MSFdata.Text)) < 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 > Then lancapnhat = check_in_out If in_out_side = True Then If Check2.Value = 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 = 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 = End If 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 If enable_drawpoint = True Then For t = 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 = 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 is rotating right." Case "command2": Button = "command2": Sta.Panels(2).Text = "Joint is rotating left." Case "command3": Button = "command3": Sta.Panels(2).Text = "Joint is rotating right." Case "command4": Button = "command4": Sta.Panels(2).Text = "Joint 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 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 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 = 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 = Button = "command1" If Check1.Value = 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 = '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 is rotating right." End If thoat: End Sub 10 For i = 91 To 116 Affine3D wireframe1.Vert(i), wireframe1.Verta(i), obit - khop1rotate CabinetProjection wireframe1.Verta(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(i, 0) = i: wireframe1.Edge(i, 1) = i + Next i For i = To wireframe1.NumEdges - If i = 64 Then End If V1 = wireframe1.Edge(i, 0) V2 = wireframe1.Edge(i, 1) P1.X = wireframe1.Vertp(V1).X P1.Y = wireframe1.Vertp(V1).Y P2.X = wireframe1.Vertp(V2).X P2.Y = wireframe1.Vertp(V2).Y khau1x(i) = xc + Round(P1.X, 0) khau1y(i) = yc + Round(P1.Y, 0) If i 64 Then Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(R, G, B) End If Next i End Sub '/////////// Private Sub khoielip2(ByVal xc As Integer, ByVal yc As Integer, ByVal zc As Integer, ByVal hoanh As Single, ByVal tung As Single, ByVal cao As Single, ByVal R, G, B As Byte) Dim wireframe1 As wireframe Dim i, scale1, V1, V2 As Integer Dim P1 As POINT2D Dim P2 As POINT2D Dim du, u du = (8 * Atn(1)) / 50 wireframe1.NumVerts = 12 wireframe1.NumEdges = 116 wireframe1.Vert(0).X = 0: wireframe1.Vert(0).Y = 0: wireframe1.Vert(0).z = -tung / zc wireframe1.Vert(1).X = hoanh: wireframe1.Vert(1).Y = 0: wireframe1.Vert(1).z = -tung / - zc wireframe1.Vert(2).X = hoanh: wireframe1.Vert(2).Y = 0: wireframe1.Vert(2).z = tung / - zc wireframe1.Vert(3).X = 0: wireframe1.Vert(3).Y = 0: wireframe1.Vert(3).z = tung / zc wireframe1.Vert(4).X = 0: wireframe1.Vert(4).Y = -cao: wireframe1.Vert(4).z = -tung / - zc 72 wireframe1.Vert(5).X = hoanh: wireframe1.Vert(5).Y = -cao: wireframe1.Vert(5).z = -tung / - zc wireframe1.Vert(6).X = hoanh: wireframe1.Vert(6).Y = -cao: wireframe1.Vert(6).z = tung / - zc wireframe1.Vert(7).X = 0: wireframe1.Vert(7).Y = -cao: wireframe1.Vert(7).z = tung / - zc wireframe1.Vert(8).X = -tung / 2: wireframe1.Vert(8).Y = 0: wireframe1.Vert(8).z = -zc wireframe1.Vert(9).X = hoanh + tung / 2: wireframe1.Vert(9).Y = 0: wireframe1.Vert(9).z = -zc wireframe1.Vert(10).X = -tung / 2: wireframe1.Vert(10).Y = -cao: wireframe1.Vert(10).z = -zc wireframe1.Vert(11).X = hoanh + tung / 2: wireframe1.Vert(11).Y = -cao: wireframe1.Vert(11).z = -zc u = 1.5707 For i = 13 To 38 wireframe1.Vert(i).X = (tung / 2) * Cos(u): wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = -(tung / 2) * Sin(u) - zc u = u + du Next i u = -1.5707 For i = 39 To 64 wireframe1.Vert(i).X = hoanh + (tung * Cos(u)) / 2: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = -(tung * Sin(u)) / - zc u = u + du Next i u = 1.5707 For i = 65 To 90 wireframe1.Vert(i).X = (tung / 2) * Cos(u): wireframe1.Vert(i).Y = -cao: wireframe1.Vert(i).z = -(tung / 2) * Sin(u) - zc u = u + du Next i u = -1.5707 For i = 91 To 116 wireframe1.Vert(i).X = hoanh + (tung * Cos(u)) / 2: wireframe1.Vert(i).Y = -cao: wireframe1.Vert(i).z = -(tung * Sin(u)) / - zc u = u + du Next i For i = To wireframe1.NumVerts TranslationForward1 wireframe1.Vert(i), wireframe1.VertTF(i) Next i 73 For i = To wireframe1.NumVerts Affine3D wireframe1.VertTF(i), wireframe1.Verta(i), obit - khop1rotate khop2rotate Next i For i = To wireframe1.NumVerts TranslationBackward1 wireframe1.Verta(i), wireframe1.VertTB(i) Next i For i = To wireframe1.NumVerts CabinetProjection wireframe1.VertTB(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(0, 0) = 0: wireframe1.Edge(0, 1) = wireframe1.Edge(1, 0) = 3: wireframe1.Edge(1, 1) = wireframe1.Edge(2, 0) = 4: wireframe1.Edge(2, 1) = wireframe1.Edge(3, 0) = 7: wireframe1.Edge(3, 1) = wireframe1.Edge(4, 0) = 8: wireframe1.Edge(4, 1) = 10 wireframe1.Edge(5, 0) = 9: wireframe1.Edge(5, 1) = 11 Next i For i = 13 To 38 TranslationForward1 wireframe1.Vert(i), wireframe1.VertTF(i) Next i For i = 13 To 38 Affine3D wireframe1.VertTF(i), wireframe1.Verta(i), obit - khop1rotate khop2rotate Next i For i = 13 To 38 TranslationBackward1 wireframe1.Verta(i), wireframe1.VertTB(i) Next i For i = 13 To 38 CabinetProjection wireframe1.VertTB(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(i, 0) = i: wireframe1.Edge(i, 1) = i + Next i For i = 39 To 64 TranslationForward1 wireframe1.Vert(i), wireframe1.VertTF(i) Next i For i = 39 To 64 Affine3D wireframe1.VertTF(i), wireframe1.Verta(i), obit - khop1rotate khop2rotate Next i For i = 39 To 64 TranslationBackward1 wireframe1.Verta(i), wireframe1.VertTB(i) Next i For i = 39 To 64 CabinetProjection wireframe1.VertTB(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(i, 0) = i: wireframe1.Edge(i, 1) = i + 74 Next i For i = 65 To 90 TranslationForward1 wireframe1.Vert(i), wireframe1.VertTF(i) Next i For i = 65 To 90 Affine3D wireframe1.VertTF(i), wireframe1.Verta(i), obit - khop1rotate khop2rotate Next i For i = 65 To 90 TranslationBackward1 wireframe1.Verta(i), wireframe1.VertTB(i) Next i For i = 65 To 90 CabinetProjection wireframe1.VertTB(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(i, 0) = i: wireframe1.Edge(i, 1) = i + Next i For i = 91 To 116 TranslationForward1 wireframe1.Vert(i), wireframe1.VertTF(i) Next i For i = 91 To 116 Affine3D wireframe1.VertTF(i), wireframe1.Verta(i), obit - khop1rotate khop2rotate Next i For i = 91 To 116 TranslationBackward1 wireframe1.Verta(i), wireframe1.VertTB(i) Next i For i = 91 To 116 CabinetProjection wireframe1.VertTB(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(i, 0) = i: wireframe1.Edge(i, 1) = i + Next i For i = To wireframe1.NumEdges - If i = 64 Then End If V1 = wireframe1.Edge(i, 0) V2 = wireframe1.Edge(i, 1) P1.X = wireframe1.Vertp(V1).X P1.Y = wireframe1.Vertp(V1).Y P2.X = wireframe1.Vertp(V2).X P2.Y = wireframe1.Vertp(V2).Y khau2x(i) = xc + Round(P1.X, 0) khau2y(i) = yc + Round(P1.Y, 0) If i 64 Then Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(R, G, B) End If 75 Next i End Sub Private Sub tructoado(ByVal xc, yc, zc As Integer, ByVal lenghx, lenghy, lenghz As Single, ByVal R1, G1, B1, R2, G2, B2 As Byte) Dim wireframe1 As wireframe Dim i, V1, V2, k, tam As Single Dim P1 As POINT2D Dim P2 As POINT2D wireframe1.NumVerts = 55 wireframe1.NumEdges = 31 wireframe1.Vert(0).X = 0: wireframe1.Vert(0).Y = 0: wireframe1.Vert(0).z = wireframe1.Vert(1).X = 50: wireframe1.Vert(1).Y = 0: wireframe1.Vert(1).z = wireframe1.Vert(2).X = 0: wireframe1.Vert(2).Y = -50: wireframe1.Vert(2).z = wireframe1.Vert(3).X = 0: wireframe1.Vert(3).Y = 0: wireframe1.Vert(3).z = 50 For i = To wireframe1.Vert(i).X = -tam * lenghx / 10: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = lenghz / tam = tam + Next i tam = For i = To 13 wireframe1.Vert(i).X = tam * lenghx / 10: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = lenghz / tam = tam + Next i tam = For i = 14 To 18 wireframe1.Vert(i).X = -lenghx / 2: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = tam * lenghz / 10 tam = tam + Next i tam = For i = 19 To 23 wireframe1.Vert(i).X = -lenghx / 2: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = -tam * lenghz / 10 tam = tam + Next i tam = For i = 24 To 28 wireframe1.Vert(i).X = -tam * lenghx / 10: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = -lenghz / 'wireframe1.Vert(i).z = -(lenghz + k) tam = tam + Next i tam = For i = 29 To 33 76 wireframe1.Vert(i).X = tam * lenghx / 10: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = -lenghz / 'wireframe1.Vert(i).z = -(lenghz + k) tam = tam + Next i tam = For i = 34 To 38 wireframe1.Vert(i).X = lenghx / 2: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = tam * lenghz / 10 tam = tam + Next i tam = For i = 39 To 43 wireframe1.Vert(i).X = lenghx / 2: wireframe1.Vert(i).Y = 0: wireframe1.Vert(i).z = -tam * lenghz / 10 tam = tam + Next i '''''''bosung wireframe1.Vert(44).X = lenghx / 20 + 7: wireframe1.Vert(44).Y = 0: wireframe1.Vert(44).z = wireframe1.Vert(45).X = lenghx / 20 + 7: wireframe1.Vert(45).Y = 0: wireframe1.Vert(45).z = -7 wireframe1.Vert(46).X = lenghx / 20 + 20: wireframe1.Vert(46).Y = 0: wireframe1.Vert(46).z = wireframe1.Vert(47).X = lenghx / 20 + 20: wireframe1.Vert(47).Y = 0: wireframe1.Vert(47).z = -7 wireframe1.Vert(48).X = 0: wireframe1.Vert(48).Y = 0: wireframe1.Vert(48).z = lenghz / 20 + 10 wireframe1.Vert(49).X = 0: wireframe1.Vert(49).Y = 0: wireframe1.Vert(49).z = lenghz / 20 + 20 wireframe1.Vert(50).X = 5: wireframe1.Vert(50).Y = 0: wireframe1.Vert(50).z = lenghz / 20 + 30 wireframe1.Vert(51).X = -5: wireframe1.Vert(51).Y = 0: wireframe1.Vert(51).z = lenghz / 20 + 30 wireframe1.Vert(52).X = 5: wireframe1.Vert(52).Y = -lenghy / 20 - 15: wireframe1.Vert(52).z = wireframe1.Vert(53).X = -5: wireframe1.Vert(53).Y = -lenghy / 20 - 15: wireframe1.Vert(53).z = wireframe1.Vert(54).X = 5: wireframe1.Vert(54).Y = -lenghy / 20 - 5: wireframe1.Vert(54).z = wireframe1.Vert(55).X = -5: wireframe1.Vert(55).Y = -lenghy / 20 - 5: wireframe1.Vert(55).z = '''''''''''''' '///////////////////////////////// For i = To wireframe1.NumVerts 77 Affine3D wireframe1.Vert(i), wireframe1.Verta(i), + obit Next i For i = To wireframe1.NumVerts CabinetProjection wireframe1.Verta(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(0, 0) = 0: wireframe1.Edge(0, 1) = wireframe1.Edge(1, 0) = 0: wireframe1.Edge(1, 1) = wireframe1.Edge(2, 0) = 0: wireframe1.Edge(2, 1) = wireframe1.Edge(24, 0) = 44: wireframe1.Edge(24, 1) = 47 wireframe1.Edge(25, 0) = 45: wireframe1.Edge(25, 1) = 46 wireframe1.Edge(26, 0) = 48: wireframe1.Edge(26, 1) = 49 wireframe1.Edge(27, 0) = 49: wireframe1.Edge(27, 1) = 50 wireframe1.Edge(28, 0) = 49: wireframe1.Edge(28, 1) = 51 wireframe1.Edge(29, 0) = 52: wireframe1.Edge(29, 1) = 53 wireframe1.Edge(30, 0) = 53: wireframe1.Edge(30, 1) = 54 wireframe1.Edge(31, 0) = 54: wireframe1.Edge(31, 1) = 55 '//////////////////////////// Next i For i = To 23 wireframe1.Edge(i, 0) = i: wireframe1.Edge(i, 1) = i + 20: Next i wireframe1.Edge(24, 0) = 44: wireframe1.Edge(24, 1) = 47 wireframe1.Edge(25, 0) = 45: wireframe1.Edge(25, 1) = 46 For i = To wireframe1.NumEdges V1 = wireframe1.Edge(i, 0) V2 = wireframe1.Edge(i, 1) P1.X = wireframe1.Vertp(V1).X P1.Y = wireframe1.Vertp(V1).Y P2.X = wireframe1.Vertp(V2).X P2.Y = wireframe1.Vertp(V2).Y If i = Or i = Or i = Or i = 24 Or i = 25 Or i = 26 Or i = 27 Or i = 28 Or i = 29 Or i = 30 Or i = 31 Then DrawWidth = Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(R1, G1, B1) Else DrawWidth = DrawStyle = Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(R2, G2, B2) End If Next i End Sub 78 Private Sub taykep(ByVal xc As Integer, ByVal yc As Integer, ByVal zc As Integer, ByVal dai, cao, rong As Single, ByVal R, G, B As Byte) Dim wireframe1 As wireframe Dim i, V1, V2 As Single Dim P1 As POINT2D Dim P2 As POINT2D wireframe1.NumVerts = wireframe1.NumEdges = wireframe1.Vert(0).X = -dai / 2: wireframe1.Vert(0).Y = 0: wireframe1.Vert(0).z = -rong / + zc wireframe1.Vert(1).X = dai / 2: wireframe1.Vert(1).Y = 0: wireframe1.Vert(1).z = -rong / + zc wireframe1.Vert(2).X = dai / 2: wireframe1.Vert(2).Y = cao: wireframe1.Vert(2).z = -rong / + zc wireframe1.Vert(3).X = -dai / 2: wireframe1.Vert(3).Y = cao: wireframe1.Vert(3).z = -rong / + zc '//////////////////// wireframe1.Vert(4).X = -dai / 2: wireframe1.Vert(4).Y = 0: wireframe1.Vert(4).z = rong / + zc wireframe1.Vert(5).X = dai / 2: wireframe1.Vert(5).Y = 0: wireframe1.Vert(5).z = rong / + zc wireframe1.Vert(6).X = dai / 2: wireframe1.Vert(6).Y = cao: wireframe1.Vert(6).z = rong / + zc wireframe1.Vert(7).X = -dai / 2: wireframe1.Vert(7).Y = cao: wireframe1.Vert(7).z = rong / + zc '///////////////////////// For i = To wireframe1.NumVerts TranslationForward2 wireframe1.Vert(i), wireframe1.VertTF(i) Next i For i = To wireframe1.NumVerts Affine3D wireframe1.VertTF(i), wireframe1.Verta(i), 90 + obit - khop1rotate khop2rotate - khop4rotate Next i For i = To wireframe1.NumVerts TranslationBackward2 wireframe1.Verta(i), wireframe1.VertTB(i) Next i For i = To wireframe1.NumVerts CabinetProjection wireframe1.VertTB(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(0, 0) = 0: wireframe1.Edge(0, 1) = wireframe1.Edge(1, 0) = 1: wireframe1.Edge(1, 1) = wireframe1.Edge(2, 0) = 0: wireframe1.Edge(2, 1) = wireframe1.Edge(3, 0) = 4: wireframe1.Edge(3, 1) = wireframe1.Edge(4, 0) = 5: wireframe1.Edge(4, 1) = wireframe1.Edge(5, 0) = 4: wireframe1.Edge(5, 1) = 79 wireframe1.Edge(6, 0) = 0: wireframe1.Edge(6, 1) = wireframe1.Edge(7, 0) = 1: wireframe1.Edge(7, 1) = wireframe1.Edge(8, 0) = 3: wireframe1.Edge(8, 1) = wireframe1.Edge(9, 0) = 2: wireframe1.Edge(9, 1) = '//////////////////////////// Next i For i = To wireframe1.NumEdges V1 = wireframe1.Edge(i, 0) V2 = wireframe1.Edge(i, 1) P1.X = wireframe1.Vertp(V1).X P1.Y = wireframe1.Vertp(V1).Y P2.X = wireframe1.Vertp(V2).X P2.Y = wireframe1.Vertp(V2).Y Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(R, G, B) Next i End Sub Private Sub DrawPoint(ByVal xc As Integer, ByVal yc As Integer, ByVal zc As Integer, ByVal mangtheox, mangtheoy, mangtheoz As Single, ByVal t As Integer) Dim wireframe1 As wireframe Dim i, V1, V2 As Single Dim P1 As POINT2D Dim P2 As POINT2D wireframe1.NumVerts = wireframe1.NumEdges = If R_point(t) = 255 Then DrawWidth = wireframe1.Vert(0).X = mangtheox + 3: wireframe1.Vert(0).Y = -mangtheoy - 3: wireframe1.Vert(0).z = -(mangtheoz + gocz) wireframe1.Vert(1).X = mangtheox - 3: wireframe1.Vert(1).Y = -mangtheoy - 3: wireframe1.Vert(1).z = -(mangtheoz + gocz) wireframe1.Vert(2).X = mangtheox + 3: wireframe1.Vert(2).Y = -mangtheoy + 3: wireframe1.Vert(2).z = -(mangtheoz + gocz) wireframe1.Vert(3).X = mangtheox - 3: wireframe1.Vert(3).Y = -mangtheoy + 3: wireframe1.Vert(3).z = -(mangtheoz + gocz) Else DrawWidth = wireframe1.Vert(0).X = mangtheox + 1: wireframe1.Vert(0).Y = -mangtheoy - 1: wireframe1.Vert(0).z = -(mangtheoz + gocz) wireframe1.Vert(1).X = mangtheox - 1: wireframe1.Vert(1).Y = -mangtheoy - 1: wireframe1.Vert(1).z = -(mangtheoz + gocz) 80 wireframe1.Vert(2).X = mangtheox + 1: wireframe1.Vert(2).Y = -mangtheoy + 1: wireframe1.Vert(2).z = -(mangtheoz + gocz) wireframe1.Vert(3).X = mangtheox - 1: wireframe1.Vert(3).Y = -mangtheoy + 1: wireframe1.Vert(3).z = -(mangtheoz + gocz) End If For i = To wireframe1.NumVerts Affine3D wireframe1.Vert(i), wireframe1.Verta(i), + obit Next i For i = To wireframe1.NumVerts CabinetProjection wireframe1.Verta(i), wireframe1.Vertp(i), rotateupdown wireframe1.Edge(0, 0) = 0: wireframe1.Edge(0, 1) = wireframe1.Edge(1, 0) = 1: wireframe1.Edge(1, 1) = Next i For i = To wireframe1.NumEdges V1 = wireframe1.Edge(i, 0) V2 = wireframe1.Edge(i, 1) P1.X = wireframe1.Vertp(V1).X P1.Y = wireframe1.Vertp(V1).Y P2.X = wireframe1.Vertp(V2).X P2.Y = wireframe1.Vertp(V2).Y If R_point(t) = 255 Then Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(255, 0, 0) If R_point(t) = Then Line (xc + P1.X, yc + P1.Y)-(xc + P2.X, yc + P2.Y), RGB(255, 255, 255) Next i End Sub Private Sub Timer7_Timer() If Abs(ProgressBar1.Value - ProgressBar1.Max) > 10 Then ProgressBar1.Value = ProgressBar1.Value + If khop1rotate > khop1_home Then khop1rotate = khop1rotate - If khop1rotate < khop1_home Then khop1rotate = khop1rotate + If khop2rotate > khop2_home Then khop2rotate = khop2rotate - If khop2rotate < khop2_home Then khop2rotate = khop2rotate + If khop4rotate > khop4_home Then khop4rotate = khop4rotate - If khop4rotate < khop4_home Then khop4rotate = khop4rotate + If high > h_home Then high = high - 3.5 If high < h_home Then high = high + 3.5 If khop1rotate < khop1_home + And khop1rotate > khop1_home - And khop2rotate < khop2_home + And khop2rotate > khop2_home - And khop4rotate < khop4_home + And khop4rotate > khop4_home - And high < h_home + 3.5 And high > h_home - 3.5 Then ProgressBar1.Value = ProgressBar1.Max 81 khop1rotate = khop1_home: khop2rotate = khop2_home: khop4rotate = khop4_home: high = h_home Timer7.Enabled = False Command11.Caption = "&Home" robothome = False Sta.Panels(2).Text = "Robot is at home." Timer1.Enabled = True End If 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(5).Text = CStr(Round(khop1rotate, 3)) Text(6).Text = CStr(Round(khop2rotate, 3)) Text(7).Text = CStr(Round(khop4rotate, 3)) 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)) Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3 If enable_drawpoint = True Then For t = To max_point DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t Next t End If End Sub Private Sub Timer8_Timer() Dim i As Byte If Button = "command1" Then khop1rotate = khop1rotate + slowquick If Button = "command2" Then khop1rotate = khop1rotate - slowquick If Button = "command3" Then khop2rotate = khop2rotate + slowquick If Button = "command4" Then khop2rotate = khop2rotate - slowquick If Button = "command5" Then khop3rotate = khop3rotate + slowquick If Button = "command6" Then khop3rotate = khop3rotate - slowquick If Button = "command7" Then high = high - slowquick If Button = "command8" Then high = high + slowquick X = (lenghelip1 - widthelip) * Cos((khop1rotate + khop2rotate) * pi / 180) + (lenghelip1 - widthelip) * Cos(khop1rotate * pi / 180) Y = (lenghelip1 - widthelip) * Sin((khop1rotate + khop2rotate) * pi / 180) + (lenghelip1 - widthelip) * Sin(khop1rotate * pi / 180) z = lenghkhoihop1 + lenghkhoihop2 + heighttru1a + * heighttru1b + * heightelip high Text1.Text = CStr(khop1rotate) Text2.Text = CStr(khop2rotate) 82 Text3.Text = CStr(khop3rotate) Text4.Text = CStr(X) Text5.Text = CStr(-Y) Text6.Text = CStr(-z) Text(1).Text = khop3rotate Text(2).Text = CStr(Round(X, 3)) Text(3).Text = CStr(Round(-Y, 3)) Text(4).Text = CStr(Round(-z, 3)) Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3 For i = To maxrow DrawPoint gocx, gocy, gocz, mangx(i), mangy(i), mangz(i), t Next i End Sub Private Sub Timer9_Timer() slowquick = Nextpoint: If noisuy(timeauto) = False Then Timer9.Interval = 10 MSFdata.col = 6: MSFdata.row = timeauto_speed: slowquick = CSng(MSFdata.Text) * 0.5 If (mangkhop1rotate(timeauto) > khop1rotate) And (mangkhop1rotate(timeauto) khop1rotate > slowquick) Then khop1rotate = khop1rotate + slowquick If (mangkhop1rotate(timeauto) < khop1rotate) And (khop1rotate mangkhop1rotate(timeauto) > slowquick) Then khop1rotate = khop1rotate - slowquick If (mangkhop2rotate(timeauto) > khop2rotate) And (mangkhop2rotate(timeauto) khop2rotate > slowquick) Then khop2rotate = khop2rotate + slowquick If (mangkhop2rotate(timeauto) < khop2rotate) And (khop2rotate mangkhop2rotate(timeauto) > slowquick) Then khop2rotate = khop2rotate - slowquick If (mangkhop4rotate(timeauto) > khop4rotate) And (mangkhop4rotate(timeauto) khop4rotate > slowquick) Then khop4rotate = khop4rotate + slowquick If (mangkhop4rotate(timeauto) < khop4rotate) And (khop4rotate mangkhop4rotate(timeauto) > slowquick) Then khop4rotate = khop4rotate - slowquick If (manghrotate(timeauto) > z) And (manghrotate(timeauto) - z > slowquick) Then high = high - slowquick If (manghrotate(timeauto) < z) And (z - manghrotate(timeauto) > slowquick) Then high = high + slowquick z = (optionform.d1_lengh_tam / 2.5 - high * scale_gra / 2.5 - 100 * scale_gra) / scale_gra Else If noisuy(timeauto - 1) = False And noisuy(timeauto) = True Then timeauto_speed = timeauto_speed + timeauto_speed = timeauto_speed - khop1rotate = mangkhop1rotate(timeauto) khop2rotate = mangkhop2rotate(timeauto) khop4rotate = mangkhop4rotate(timeauto) 83 z = manghrotate(timeauto): high = CSng(optionform.Text(5).Text) - z * 2.5 End If 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 Text(5).Text = CStr(Round(khop1rotate, 3)) Text(6).Text = CStr(Round(khop2rotate, 3)) Text(7).Text = CStr(Round(khop4rotate, 3)) 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)) If tool(timeauto) = "Open" Or tool(timeauto) = " Open" Then M5C = False Else M5C = True End If Drawrobot R1, G1, B1, R2, G2, B2, R3, G3, B3 If enable_drawpoint = True Then For t = To timeauto DrawPoint gocx, gocy, gocz, mangx(t), mangy(t), mangz(t), t Next t End If If (mangkhop1rotate(timeauto) - khop1rotate)