OK das heir ist der code:
Code:
$prog , 255 , &B11011001 , 'Quarz an / Teiler aus / Jtag aus
$regfile = "m2560def.dat"
$hwstack = 82 '80
$framesize = 68 ' 64
$swstack = 68 '44
$crystal = 16000000 'Quarzfrequenz
$baud = 9600
'################# USB ###################################
Config Pine.5 = Input
USB Alias Pine.5 'Ist 1 wenn USB angeschlossen
Config Com4 = 9600 , Synchrone = 0 , Parity = None , Stopbits = 1 , Databits = 8 , Clockpol = 0
Open "com4:" For Binary As #4 'USB Buchse
'##################### Analoger Port #####################
Config Adc = Single , Prescaler = Auto , Reference = Avcc
'###################### PWM ##############################
Config Timer1 = Pwm , Pwm = 8 , Compare B Pwm = Clear Down , Compare A Pwm = Clear Down , Prescale = 64
'############################ Variablen ##################
Dim Winkel_g1_soll As Word
Dim Winkel_g2_soll As Word
Dim Winkel_g3_soll As Word
Dim Winkel_g4_soll As Word
Dim Winkel_g5_soll As Word
Dim Winkel_g6_soll As Word
Dim Winkel_g1_ist As Word
Dim Winkel_g2_ist As Word
Dim Winkel_g3_ist As Word
Dim Winkel_g4_ist As Word
Dim Winkel_g5_ist As Word
Dim Winkel_g6_ist As Word
Dim Winkel_differenz As Integer
Dim Winkel_langsam As Word
Dim G11 As Bit
Dim G12 As Bit
Dim G21 As Bit
Dim G22 As Bit
Dim G31 As Bit
Dim G32 As Bit
Dim G41 As Bit
Dim G42 As Bit
Dim G51 As Bit
Dim G52 As Bit
Dim G61 As Bit
Dim G62 As Bit
Dim Pos_erreicht As Bit
Dim Usb_eingang As Byte
'############################### PORTS ###################
'----------------------led------------------------------
Config Pind.5 = Output
Led Alias Portd.5
'------------------ Motor 6 (greifer) -------------------
Config Pinc.0 = Output
Config Pinc.1 = Output
Config Pinc.2 = Output
M_6_v Alias Portc.0
M_6_e Alias Portc.1
M_6_r Alias Portc.2
'------------------- Motor 5 (drehgelenk) ---------------
Config Pinc.4 = Output
Config Pinb.6 = Output ' pwm
Config Pinc.5 = Output
M_5_v Alias Portc.4
M_5_e Alias Portb.6
M_5_r Alias Portc.5
'------------------ Motor 4 (handgelenk) ----------------
Config Pinc.6 = Output
Config Pinb.5 = Output
Config Pinj.3 = Output
M_4_v Alias Portc.6
M_4_e Alias Portb.5
M_4_r Alias Portj.3
'------------------- Motor 3 (unterarm) -----------------
Config Pinj.4 = Output
Config Pinj.5 = Output
Config Pinj.6 = Output
M_3_v Alias Portj.4
M_3_e Alias Portj.5
M_3_r Alias Portj.6
'------------------- Motor 2 (schulter) -----------------
'------------------- Motor 1 (gesamtdrehen) -------------
'################### SUB DECLARATION ####################
Declare Sub Greifer_auf()
Declare Sub Greifer_zu()
Declare Sub Greifer_stop()
Declare Sub Hand_drehen_links(byval V As Byte)
Declare Sub Hand_drehen_rechts(byval V As Byte)
Declare Sub Hand_drehen_stop()
Declare Sub Hand_drehen_halten()
Declare Sub Gelenk_4_hoch(byval V As Byte)
Declare Sub Gelenk_4_runter(byval V As Byte)
Declare Sub Gelenk_4_stop()
Declare Sub Gelenk_3_hoch()
Declare Sub Gelenk_3_runter()
Declare Sub Gelenk_3_stop()
Declare Sub Gelenk_2_hoch()
Declare Sub Gelenk_2_runter()
Declare Sub Gelenk_2_stop()
Declare Sub Gelenk_1_links()
Declare Sub Gelenk_1_rechts()
Declare Sub Gelenk_1_stop()
Declare Sub Gelenk_1_gehe(winkel_g1_soll As Word )
Declare Sub Gelenk_2_gehe(winkel_g2_soll As Word )
Declare Sub Gelenk_3_gehe(winkel_g3_soll As Word )
Declare Sub Gelenk_4_gehe(winkel_g4_soll As Word )
Declare Sub Gelenk_5_gehe(winkel_g5_soll As Word )
Declare Sub Gelenk_6_gehe(winkel_g6_soll As Word )
Declare Sub Empfange_sollwinkel()
Declare Sub Gehe_position(winkel_g1_soll As Word , Winkel_g2_soll As Word , Winkel_g3_soll As Word , Winkel_g4_soll As Word , Winkel_g5_soll As Word , Winkel_g6_soll As Word )
Declare Sub Analogeportslesen()
'########################################################
'##################### PROG #############################
'########################################################
'Usb_eingang = Waitkey(#4) Print #4 , "Winkel_g5_soll :" ; Winkel_g5_soll
' Winkel_g5_ist = Getadc(0)
Start Adc
Do
Winkel_g1_soll = 0
Winkel_g2_soll = 0
Winkel_g3_soll = 0
Winkel_g4_soll = 0
Winkel_g5_soll = 0
Winkel_g6_soll = 0
Winkel_g1_ist = 0
Winkel_g2_ist = 0
Winkel_g3_ist = 0
Winkel_g4_ist = 0
Winkel_g5_ist = 0
Winkel_g6_ist = 0
Winkel_langsam = 30
Pos_erreicht = 0
Led = 1
Waitms 100
Led = 0
Waitms 100
Led = 1
Waitms 100
Led = 0
Waitms 100
'----------------------------------------
Usb_eingang = Waitkey(#4)
Select Case Usb_eingang
Case "p"
Empfange_sollwinkel
Call Gehe_position(winkel_g1_soll , Winkel_g2_soll , Winkel_g3_soll , Winkel_g4_soll , Winkel_g5_soll , Winkel_g6_soll)
Case "a"
Analogeportslesen
End Select
Loop
End
'########################################################
'################## END PROG ############################
'########################################################
'###################### SUBS ###########################
'------------greifer---------
Sub Greifer_auf()
M_6_v = 0
M_6_e = 1
M_6_r = 1
Led = 1
End Sub
Sub Greifer_zu()
M_6_v = 1
M_6_e = 1
M_6_r = 0
Led = 1
End Sub
Sub Greifer_stop()
M_6_v = 0
M_6_e = 0
M_6_r = 0
Led = 0
End Sub
'----------drehgelenk---------
Sub Hand_drehen_links(byval V As Byte)
M_5_v = 1
Compare1b = V
M_5_r = 0
Led = 1
End Sub
Sub Hand_drehen_rechts(byval V As Byte)
M_5_v = 0
Compare1b = V
M_5_r = 1
Led = 1
End Sub
Sub Hand_drehen_stop()
M_5_v = 0
M_5_e = 0
M_5_r = 0
Led = 0
End Sub
Sub Hand_drehen_halten()
End Sub
'-----------gelenk4---------
Sub Gelenk_4_hoch(byval V As Byte)
M_4_v = 0
Compare1a = V
M_4_r = 1
Led = 1
End Sub
Sub Gelenk_4_runter(byval V As Byte)
M_4_v = 1
Compare1a = V
M_4_r = 0
Led = 1
End Sub
Sub Gelenk_4_stop()
M_4_v = 0
M_4_e = 0
M_4_r = 0
Led = 0
End Sub
'-----------gelenk3-----------
Sub Gelenk_3_hoch()
M_3_v = 0
M_3_e = 1
M_3_r = 1
Led = 1
End Sub
Sub Gelenk_3_runter()
M_3_v = 1
M_3_e = 1
M_3_r = 0
Led = 1
End Sub
Sub Gelenk_3_stop()
M_3_v = 0
M_3_e = 0
M_3_r = 0
Led = 0
End Sub
'-----------gelenk2-----------
Sub Gelenk_2_hoch()
Led = 1
End Sub
Sub Gelenk_2_runter()
Led = 1
End Sub
Sub Gelenk_2_stop()
Led = 0
End Sub
'-----------gelenk1-----------
Sub Gelenk_1_links()
Led = 1
End Sub
Sub Gelenk_1_rechts()
Led = 1
End Sub
Sub Gelenk_1_stop()
Led = 0
End Sub
'######### ABSOLUTWINKEL SUBS ######
Sub Gelenk_1_gehe(winkel_g1_soll As Word )
End Sub
Sub Gelenk_2_gehe(winkel_g2_soll As Word )
End Sub
Sub Gelenk_3_gehe(winkel_g3_soll As Word )
Winkel_g3_ist = Getadc(2)
'Winkel_differenz = Winkel_g3_soll - Winkel_g3_ist
'Winkel_differenz = Abs(winkel_differenz)
If Winkel_g3_ist < Winkel_g3_soll Then
Call Gelenk_3_runter()
G31 = 1
'Elseif Winkel_g4_ist > Winkel_g4_soll And Winkel_differenz <= Winkel_langsam Then
' Call Gelenk_4_runter(80)
' G31 = 1
Elseif Winkel_g3_ist > Winkel_g3_soll Then
Call Gelenk_3_hoch()
G32 = 1
'Elseif Winkel_g4_ist < Winkel_g4_soll And Winkel_differenz <= Winkel_langsam Then
'Call Gelenk_4_hoch(80)
'G32 = 1
Else
G31 = 1
G32 = 1
End If
End Sub
Sub Gelenk_4_gehe(winkel_g4_soll As Word ) ' Fahre Gelenk 4
Winkel_g4_ist = Getadc(1)
Winkel_differenz = Winkel_g4_soll - Winkel_g4_ist
Winkel_differenz = Abs(winkel_differenz)
If Winkel_g4_ist > Winkel_g4_soll And Winkel_differenz > Winkel_langsam Then
Call Gelenk_4_runter(255)
G41 = 1
Elseif Winkel_g4_ist > Winkel_g4_soll And Winkel_differenz <= Winkel_langsam Then
Call Gelenk_4_runter(80)
G41 = 1
Elseif Winkel_g4_ist < Winkel_g4_soll And Winkel_differenz > Winkel_langsam Then
Call Gelenk_4_hoch(255)
G42 = 1
Elseif Winkel_g4_ist < Winkel_g4_soll And Winkel_differenz <= Winkel_langsam Then
Call Gelenk_4_hoch(120)
G42 = 1
Else
G41 = 1
G42 = 1
End If
End Sub
Sub Gelenk_5_gehe(winkel_g5_soll As Word ) ' Fahre Gelennk 5
Winkel_g5_ist = Getadc(4)
Winkel_differenz = Winkel_g5_soll - Winkel_g5_ist
Winkel_differenz = Abs(winkel_differenz)
If Winkel_g5_ist > Winkel_g5_soll And Winkel_differenz > Winkel_langsam Then
Call Hand_drehen_links(255)
G51 = 1
Elseif Winkel_g5_ist > Winkel_g5_soll And Winkel_differenz <= Winkel_langsam Then
Call Hand_drehen_links(70)
G51 = 1
Elseif Winkel_g5_ist < Winkel_g5_soll And Winkel_differenz > Winkel_langsam Then
Call Hand_drehen_rechts(255)
G52 = 1
Elseif Winkel_g5_ist < Winkel_g5_soll And Winkel_differenz <= Winkel_langsam Then
Call Hand_drehen_rechts(70)
G52 = 1
Else
G51 = 1
G52 = 1
End If
End Sub
Sub Gelenk_6_gehe(winkel_g6_soll As Word )
End Sub
Sub Empfange_sollwinkel() ' USB empfang der SollWinkel
Local Eingang As Byte
Local E As Word
Winkel_g3_ist = Getadc(2)
Winkel_g4_ist = Getadc(1)
Winkel_g5_ist = Getadc(4)
Winkel_g3_soll = Winkel_g3_ist
Winkel_g4_soll = Winkel_g4_ist
Winkel_g5_soll = Winkel_g5_ist
Eingang = Waitkey(#4)
If Eingang = "e" Then
Eingang = Waitkey(#4)
E = Eingang - 48
Winkel_g3_soll = E * 1000
Eingang = Waitkey(#4)
E = Eingang - 48
E = E * 100
Winkel_g3_soll = Winkel_g3_soll + E
Eingang = Waitkey(#4)
E = Eingang - 48
E = E * 10
Winkel_g3_soll = Winkel_g3_soll + E
Eingang = Waitkey(#4)
E = Eingang - 48
Winkel_g3_soll = Winkel_g3_soll + E
End If
Eingang = Waitkey(#4)
If Eingang = "r" Then
Eingang = Waitkey(#4)
E = Eingang - 48
Winkel_g4_soll = E * 1000
Eingang = Waitkey(#4)
E = Eingang - 48
E = E * 100
Winkel_g4_soll = Winkel_g4_soll + E
Eingang = Waitkey(#4)
E = Eingang - 48
E = E * 10
Winkel_g4_soll = Winkel_g4_soll + E
Eingang = Waitkey(#4)
E = Eingang - 48
Winkel_g4_soll = Winkel_g4_soll + E
End If
Eingang = Waitkey(#4)
If Eingang = "t" Then
Eingang = Waitkey(#4)
E = Eingang - 48
Winkel_g5_soll = E * 1000
Eingang = Waitkey(#4)
E = Eingang - 48
E = E * 100
Winkel_g5_soll = Winkel_g5_soll + E
Eingang = Waitkey(#4)
E = Eingang - 48
E = E * 10
Winkel_g5_soll = Winkel_g5_soll + E
Eingang = Waitkey(#4)
E = Eingang - 48
Winkel_g5_soll = Winkel_g5_soll + E
End If
End Sub
' SollWinkel Anfahren
Sub Gehe_position(winkel_g1_soll As Word , Winkel_g2_soll As Word , Winkel_g3_soll As Word , Winkel_g4_soll As Word , Winkel_g5_soll As Word , Winkel_g6_soll As Word )
G11 = 1 : G12 = 1 : G21 = 1 : G22 = 1 : G31 = 0 : G32 = 0
G41 = 0 : G42 = 0 : G51 = 0 : G52 = 0 : G61 = 1 : G62 = 1
Pos_erreicht = 0
While Pos_erreicht < 1
Print #4 , "3ist : " ; Winkel_g3_ist ; " 3soll : " ; Winkel_g3_soll
If G11 = 1 And G12 = 1 And G21 = 1 And G22 = 1 And G31 = 1 And G32 = 1 And G41 = 1 And G42 = 1 And G51 = 1 And G52 = 1 And G61 = 1 And G62 = 1 Then Pos_erreicht = 1 Else Pos_erreicht = 0
If G31 = 1 And G32 = 1 Then
Gelenk_3_stop
Else
Call Gelenk_3_gehe(winkel_g3_soll)
End If
If G41 = 1 And G42 = 1 Then
Gelenk_4_stop
Else
Call Gelenk_4_gehe(winkel_g4_soll)
End If
If G51 = 1 And G52 = 1 Then
Hand_drehen_stop
Else
Call Gelenk_5_gehe(winkel_g5_soll)
End If
Wend
Greifer_stop
Hand_drehen_stop
Gelenk_4_stop
Gelenk_3_stop
Gelenk_2_stop
Gelenk_1_stop
Waitms 1000
Winkel_g4_ist = Getadc(1)
Winkel_g5_ist = Getadc(4)
Print #4 , ", 3i :" ; Winkel_g3_ist ; ", 3s :" ; Winkel_g3_soll ; ", 4i :" ; Winkel_g4_ist ; ", 4s :" ; Winkel_g4_soll
End Sub
Sub Analogeportslesen()
Local W1 As Word
Local W2 As Word
Local W3 As Word
Local W4 As Word
Local W5 As Word
Local W6 As Word
Local W7 As Word
Local W8 As Word
Local W9 As Word
Local W10 As Word
Local W11 As Word
Local W12 As Word
Local W13 As Word
Local W14 As Word
Local W15 As Word
Local W16 As Word
Usb_eingang = Waitkey(#4)
If Usb_eingang = "a" Then
W1 = Getadc(0)
W2 = Getadc(1)
W3 = Getadc(2)
W4 = Getadc(3)
W5 = Getadc(4)
W6 = Getadc(5)
W7 = Getadc(6)
W8 = Getadc(7)
W9 = Getadc(8)
W10 = Getadc(9)
W11 = Getadc(10)
W12 = Getadc(11)
W13 = Getadc(12)
W14 = Getadc(13)
W15 = Getadc(14)
W16 = Getadc(15)
Print #4 , W1 ; " " ; W2 ; " " ; W3 ; " " ; W4 ; " " ; W5 ; " " ; W6 ; " " ; W7 ; " " ; W8
Waitms 800
End If
If Usb_eingang = "s" Then
W1 = Getadc(0)
W2 = Getadc(1)
W3 = Getadc(2)
W4 = Getadc(3)
W5 = Getadc(4)
W6 = Getadc(5)
W7 = Getadc(6)
W8 = Getadc(7)
W9 = Getadc(8)
W10 = Getadc(9)
W11 = Getadc(10)
W12 = Getadc(11)
W13 = Getadc(12)
W14 = Getadc(13)
W15 = Getadc(14)
W16 = Getadc(15)
Print #4 , W9 ; " " ; W10 ; " " ; W11 ; " " ; W12 ; " " ; W13 ; " " ; W14 ; " " ; W15 ; " " ; W16
End If
End Sub
das ganze hat dann noch ne benutzeroberfläche im rechner, da muss man dann immer den anstuerbuchstaben fuer das gelenk und den wert senden, die positionen kann man auch ueber teachin anfahren mit pfeilatsten und dann speichern und so das programm zusammenbaeun..
Lesezeichen