///PIC mikrovaldiklio BASIC kalba parašyta programa //Programos pradžia //RS-232 ryšio nustatymai Hseropen 9600 WaitMs 1000 //PIC išvadų nustatymai PORTA = 0 ADCON1 = 0x06 TRISA = %11110000 PORTB = 0 TRISB = 0 // Simbolių priskyrimas Symbol tx = RA.2 Symbol rx = RA.3 rx = 1 tx = 1 Symbol prf = RA.0 Symbol strobe = RA.1 Symbol base = RB.0 Symbol shoulder = RB.1 Symbol elbow = RB.2 Symbol rgrip = RB.4 Symbol lgrip = RB.3 Symbol grip = RB.5 // Deklaracijos Dim delay As Word Dim e As Word Dim e1 As Byte Dim i As Byte Dim i2 As Byte Dim i3 As Byte Dim i4 As Byte Dim s_home As Word Dim e_home As Word Dim lg_home As Word Dim g_home As Word Dim ba_max As Word Dim sh_max As Word Dim e_max As Word Dim lg_max As Word Dim g_max As Word Dim g_ba As Word Dim g_sh As Word Dim g_e As Word Dim g_lg As Word Dim g_rg As Word Dim g_g As Word Dim g_ba_max As Word Dim g_sh_max As Word Dim g_e_max As Word Dim g_lg_max As Word Dim g_g_max As Word Dim sum As Word Dim sums As Word Dim sume As Word Dim sumlg As Word Dim sumg As Word Dim stepb_p As Word Dim stepb_m As Word Dim steps_p As Word Dim steps_m As Word Dim stepe_p As Word Dim stepe_m As Word Dim steplg_p As Word Dim steplg_m As Word Dim stepg_p As Word Dim stepg_m As Word //Konstantų priskyrimas delay = 3 s_home = 1260 e_home = 520 ba_max = 2840 sh_max = 2520 e_max = 1040 g_sh = 1260 g_e = 520 g_ba = 0 stepb_p = 0 stepb_m = 0 steps_p = 0 steps_m = 0 stepe_p = 0 stepe_m = 0 // Pagrindinė programa main: rx = 1 Hserout "Done...", CrLf Hserin i // atkodavimas iš ASCII kodo ir patikrinimas kokia valdymo šaka buvo pasirinkta i = i - 48 If i = 2 Then Goto dataread Endif If i = 1 Then Hserin i e1 = i - 48 Goto programa Endif If i = 3 Then Goto setup Else Goto main Endif // Rankinis Cyber310 valdymas dataread: rx = 1 tx = 0 // gaunamas 20 baitų kodas, atkoduojama į 5 koordinates Hserin i i = i - 48 Hserin i2 i2 = i2 - 48 Hserin i3 i3 = i3 - 48 Hserin i4 i4 = i4 - 48 sum = i * 1000 + i2 * 100 + i3 * 10 + i4 Hserin i i = i - 48 Hserin i2 i2 = i2 - 48 Hserin i3 i3 = i3 - 48 Hserin i4 i4 = i4 - 48 sums = i * 1000 + i2 * 100 + i3 * 10 + i4 Hserin i i = i - 48 Hserin i2 i2 = i2 - 48 Hserin i3 i3 = i3 - 48 Hserin i4 i4 = i4 - 48 sume = i * 1000 + i2 * 100 + i3 * 10 + i4 Hserin i i = i - 48 Hserin i2 i2 = i2 - 48 Hserin i3 i3 = i3 - 48 Hserin i4 i4 = i4 - 48 sumlg = i * 1000 + i2 * 100 + i3 * 10 + i4 Hserin i i = i - 48 Hserin i2 i2 = i2 - 48 Hserin i3 i3 = i3 - 48 Hserin i4 i4 = i4 - 48 sumg = i * 1000 + i2 * 100 + i3 * 10 + i4 rx = 0 Hserout "Please wait...", CrLf Goto base_move // Bazės variklio krypties nustatymas tx = 1 base_move: If g_ba > sum Then stepb_p = g_ba - sum If g_ba < stepb_p Then Goto error1 Endif Goto base_plius Endif If g_ba < sum Then stepb_m = sum - g_ba g_ba_max = ba_max - g_ba If g_ba_max < stepb_m Then Goto error1 Endif Goto base_minus Endif Goto elbow_move base_plius: e = g_ba strobe = 1 strobe = 0 base = 1 While e <> sum e = e - 1 g_ba = g_ba - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend base = 0 Goto elbow_move base_minus: e = g_ba base = 1 strobe = 1 strobe = 0 While e <> sum e = e + 1 g_ba = g_ba + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend base = 0 e = 0 Goto elbow_move // Alkųnės variklio krypties nustatymas ir pasukimas elbow_move: If g_e > sume Then stepe_p = g_e - sume If g_e < stepe_p Then Goto error1 Endif Goto elbow_plius Endif If g_e < sume Then stepe_m = sume - g_e g_e_max = e_max - g_e If g_e_max < stepe_m Then Goto error1 Endif Goto elbow_minus Endif Goto shoulder_move elbow_plius: e = g_e elbow = 1 strobe = 1 strobe = 0 While e <> sume e = e - 1 g_e = g_e - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend elbow = 0 Goto shoulder_move elbow_minus: e = g_e strobe = 1 strobe = 0 elbow = 1 While e <> sume e = e + 1 g_e = g_e + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend elbow = 0 e = 0 Goto shoulder_move // Peties variklio krypties nustatymas ir pasukimas shoulder_move: If g_sh > sums Then steps_p = g_sh - sums If g_sh < steps_p Then Goto error3 Endif Goto shoulder_plius Endif If g_sh < sums Then steps_m = sums - g_sh g_sh_max = sh_max - g_sh If g_sh_max < steps_m Then Goto error3 Endif Goto shoulder_minus Endif Goto lrgrip_move shoulder_plius: e = g_sh strobe = 1 strobe = 0 shoulder = 1 While e <> sums e = e - 1 g_sh = g_sh - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend shoulder = 0 tx = 0 Goto lrgrip_move shoulder_minus: e = g_sh shoulder = 1 strobe = 1 strobe = 0 While e <> sums e = e + 1 g_sh = g_sh + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend shoulder = 0 e = 0 tx = 0 Goto lrgrip_move //Kairio ir dešinio griebtuvo variklių krypties nustatymas ir pasukimas lrgrip_move: If g_lrg > sumlrg Then steplrg_p = g_lrg - sumlrg If g_lrg < steplrg_p Then Goto error4 Endif Goto lrgrip_plius Endif If g_lrg < sumlrg Then steplrg_m = sumlrg - g_lrg g_lrg_max = lrg_max - g_lrg If g_lrg_max < steplrg_m Then Goto error4 Endif Goto lrgrip_minus Endif Goto main lrgrip_plius: e = g_lrg strobe = 1 strobe = 0 'shoulder = 1 While e <> sumlrg e = e - 1 g_lrg = g_lrg - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend 'shoulder = 0 tx = 0 Goto grip_move lrgrip_minus: e = g_lrg 'shoulder = 1 strobe = 1 strobe = 0 While e <> sumlrg e = e + 1 g_lrg = g_lrg + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend 'shoulder = 0 e = 0 tx = 0 Goto grip_move //Griebtuvo variklio krypties nustatymas ir pasukimas grip_move: If g_g > sumg Then stepg_p = g_g - sumg If g_g < stepg_p Then Goto error5 Endif Goto grip_plius Endif If g_g < sumg Then stepg_m = sumg - g_g g_g_max = g_max - g_g If g_g_max < stepg_m Then Goto error5 Endif Goto grip_minus Endif Goto main grip_plius: e = g_g strobe = 1 strobe = 0 grip = 1 While e <> sumg e = e - 1 g_g = g_g - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend grip = 0 tx = 0 Goto main grip_minus: e = g_g grip = 1 strobe = 1 strobe = 0 While e <> sumg e = e + 1 g_g = g_g + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend grip = 0 e = 0 tx = 0 Goto main // Automatinis valdymas programa: Hserout "Please wait...", CrLf // Visų variklių patikrinimas ar varikiai yra starto padėtyje // pasukamas bazės variklis į starto poziciją tx = 1 base_move1: If g_ba > 0 Then Goto base_plius1 Endif Goto elbow_move1 base_plius1: e = g_ba strobe = 1 strobe = 0 base = 1 While e <> 0 e = e - 1 g_ba = g_ba - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend base = 0 Goto elbow_move1 base_minus1: e = g_ba base = 1 strobe = 1 strobe = 0 While e <> 0 e = e + 1 g_ba = g_ba + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend base = 0 e = 0 Goto elbow_move1 // pasukamas alkūnės variklis į starto poziciją elbow_move1: If g_e > e_home Then Goto elbow_plius1 Endif If g_e < e_home Then Goto elbow_minus1 Endif Goto shoulder_move1 elbow_plius1: e = g_e elbow = 1 strobe = 1 strobe = 0 While e <> e_home e = e - 1 g_e = g_e - 1 g_lrg = g_lrg + lrg prf = 1 WaitMs delay prf = 0 WaitMs delay Wend elbow = 0 Goto shoulder_move1 elbow_minus1: e = g_e strobe = 1 strobe = 0 elbow = 1 While e <> e_home e = e + 1 g_e = g_e + 1 g_lrg = g_lrg - lrg prf = 1 WaitMs delay prf = 0 WaitMs delay Wend elbow = 0 e = 0 Goto shoulder_move1 // pasukamas peties variklis į starto poziciją shoulder_move1: If g_sh > s_home Then Goto shoulder_plius1 Endif If g_sh < s_home Then Goto shoulder_minus1 Endif Goto lrgrip_move shoulder_plius1: e = g_sh strobe = 1 strobe = 0 shoulder = 1 While e <> s_home e = e - 1 g_sh = g_sh - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend shoulder = 0 tx = 0 Goto lrgrip_move shoulder_minus1: e = g_sh shoulder = 1 strobe = 1 strobe = 0 While e <> s_home e = e + 1 g_sh = g_sh + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend shoulder = 0 e = 0 tx = 0 Goto lrgrip_move // pasukami kairys ir dešinys griebtuvo varikliai į starto poziciją lrgrip_move1: If g_lrg > lrg_home Then Goto lrgrip_plius1 Endif If g_lrg < lrg_home Then Goto lrgrip_minus1 Endif Goto main lrgrip_plius1: e = g_lrg strobe = 1 strobe = 0 'shoulder = 1 While e <> lrg_home e = e - 1 g_lrg = g_lrg - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend 'shoulder = 0 tx = 0 Goto grip_move1 lrgrip_minus1: e = g_lrg 'shoulder = 1 strobe = 1 strobe = 0 While e <> lrg_home e = e + 1 g_lrg = g_lrg + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend 'shoulder = 0 e = 0 tx = 0 Goto grip_move1 // pasukamas griebtuvo variklis į starto poziciją grip_move1: If g_g > g_home Then Goto grip_plius1 Endif If g_g < g_home Then Goto grip_minus1 Endif Goto auto grip_plius1: e = g_g strobe = 1 strobe = 0 'shoulder = 1 While e <> g_home e = e - 1 g_g = g_g - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend 'shoulder = 0 tx = 0 Goto auto grip_minus1: e = g_g 'shoulder = 1 strobe = 1 strobe = 0 While e <> g_home e = e + 1 g_g = g_g + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend 'shoulder = 0 // Vykdomas detalės pernešimas auto: tx = 1 While e1 <> 0 e1 = e1 - 1 WaitMs 1000 e = 0 strobe = 1 strobe = 0 base = 1 strobe = 1 strobe = 0 elbow = 1 shoulder = 1 While e <> 520 e = e + 1 g_ba = g_ba + 1 g_e = g_e + 1 g_sh = g_sh - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend elbow = 0 e = 0 While e <> 205 e = e + 1 g_ba = g_ba + 1 g_sh = g_sh - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend base = 0 e = 0 While e <> 235 e = e + 1 g_sh = g_sh - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend shoulder = 0 elbow = 1 rgrip = 1 lgrip = 1 strobe = 1 strobe = 0 rgrip = 0 lgrip = 0 e = 0 While e <> 180 e = e + 1 g_e = g_e - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 rgrip = 1 lgrip = 1 While e <> 107 e = e + 1 g_e = g_e - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend lgrip = 0 rgrip = 0 shoulder = 1 e = 0 While e <> 13 e = e + 1 g_sh = g_sh - 1 g_e = g_e - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 elbow = 0 While e <> 42 g_sh = g_sh - 1 e = e + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 shoulder = 0 grip = 1 While e <> 180 e = e + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 grip = 0 shoulder = 1 strobe = 1 strobe = 0 While e <> 20 e = e + 1 g_sh = g_sh + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 shoulder = 0 strobe = 1 strobe = 0 base = 1 strobe = 1 strobe = 0 While e <> 725 e = e + 1 g_ba = g_ba + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 base = 0 shoulder = 1 While e <> 25 e = e + 1 g_sh = g_sh - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 shoulder = 0 ''== grip = 1 strobe = 1 strobe = 0 While e <> 180 e = e + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 grip = 0 shoulder = 1 strobe = 1 strobe = 0 elbow = 1 While e <> 107 e = e + 1 g_sh = g_sh + 1 g_e = g_e + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend strobe = 1 strobe = 0 shoulder = 1 elbow = 1 strobe = 1 strobe = 0 elbow = 0 base = 1 e = 0 While e <> 915 e = e + 1 g_sh = g_sh + 1 g_ba = g_ba - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend shoulder = 0 elbow = 1 lgrip = 1 rgrip = 1 e = 0 While e <> 107 e = e + 1 g_ba = g_ba - 1 g_e = g_e - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend lgrip = 0 rgrip = 0 e = 0 While e <> 222 e = e + 1 g_ba = g_ba - 1 g_e = g_e - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend elbow = 0 e = 0 While e <> 206 e = e + 1 g_ba = g_ba - 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend base = 0 goto main //Pozicionavimas setup: Hserout "Please wait...", CrLf rx = 0 e = g_sh shoulder = 1 strobe = 1 strobe = 0 While e <> s_home e = e + 1 g_sh = g_sh + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 shoulder = 0 e = g_e strobe = 1 strobe = 0 elbow = 1 While e <> e_home e = e + 1 g_e = g_e + 1 prf = 1 WaitMs delay prf = 0 WaitMs delay Wend e = 0 elbow = 0 // Klaidų patikra error1: 'Hserout "Error base", CrLf WaitMs 2000 Goto main error2: 'Hserout "Error shoulder", CrLf WaitMs 2000 Goto main error3: 'Hserout "Error elbow", CrLf WaitMs 2000 Goto main error4: 'Hserout "Error lrgip", CrLf WaitMs 2000 Goto main error5: 'Hserout "Error grip", CrLf WaitMs 2000 Goto main