code is folgender:
Code:
'###################################################
' Greifertest'
'###################################################
'Diese Anweisung setzt die Fusebits automatisch korrekt (Syntax $PROG LB, FB , FBH , FBX )
$prog , 255 , &B11011001 , 'Quarz an / Teiler aus / Jtag aus
$regfile = "m2560def.dat"
$hwstack = 82 '80
$framesize = 68 ' 64
$swstack = 68 '44
$crystal = 16000000 'Quarzfrequenz
'################# 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
$baud = 9600
'Dim I As Integer
'Dim U As Integer
Dim Usb_eingang As Byte
'############################### PORTS ##############################
'----------------------led------------------------------
Config Pind.5 = Output
Led Alias Portd.5
'-------------------Analog Port--------------------------
Config Adc = Single , Prescaler = Auto , Reference = Avcc
'------------------ 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.3 = Output
Config Pinc.4 = Output
Config Pinc.5 = Output
M_5_v Alias Portc.3
M_5_e Alias Portc.4
M_5_r Alias Portc.5
'------------------ Motor 4 (handgelenk) ----------------
Config Pinc.6 = Output
Config Pinc.7 = Output
Config Pinj.3 = Output
M_4_v Alias Portc.6
M_4_e Alias Portc.7
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()
Declare Sub Hand_drehen_rechts()
Declare Sub Hand_drehen_stop()
Declare Sub Gelenk_4_hoch()
Declare Sub Gelenk_4_runter()
Declare Sub Gelenk_4_stop()
Declare Sub Gelenk_3_hoch()
Declare Sub Gelenk_3_runter()
Declare Sub Gelenk_3_stop()
Declare Sub Gehe_position()
'############################################################
'##################### PROG #################################
'############################################################
Do
'Usb_eingang = Waitkey(#4)
'Select Case Usb_eingang
'---------manuele steuerung----------
'Case "q" :
' Greifer_auf
'Case "w" :
' Greifer_zu
'Case "e" :
' Greifer_stop
'Case "a"
' Hand_drehen_links
'Case "s"
' Hand_drehen_rechts
'Case "d"
' Hand_drehen_stop
'Case "y" :
' Gelenk_4_hoch
'Case "x" :
' Gelenk_4_runter
'Case "c" :
' Gelenk_4_stop
'Case "r" :
' Gelenk_3_hoch
'Case "t" :
' Gelenk_3_runter
'Case "z" :
' Gelenk_3_stop
'Case Else
'End Select
Gehe_position
Loop
End
'###################### Functionen########################
Sub Gehe_position() As Byte
Local Pos_erreicht As Byte
Local Winkel_g1_soll As Word
Local Winkel_g2_soll As Word
Local Winkel_g3_soll As Word
Local Winkel_g4_soll As Word
Local Winkel_g5_soll As Word
Local Winkel_g6_soll As Word
Local Winkel_g1_ist As Word
Local Winkel_g2_ist As Word
Local Winkel_g3_ist As Word
Local Winkel_g4_ist As Word
Local Winkel_g5_ist As Word
Local Winkel_g6_ist As Word
Local I_langsam As Byte
Local Stopwinkel As Byte
Local Winkelvergleich As Word
Local E As Word
Local B As Word
Local C As Word
Local D As Word
Local G_11 As Byte
Local G_12 As Byte
Local G_21 As Byte
Local G_22 As Byte
Local G_31 As Byte
Local G_32 As Byte
Local G_41 As Byte
Local G_42 As Byte
Local G_51 As Byte
Local G_52 As Byte
Local G_61 As Byte
Local G_62 As Byte
G_11 = 0
G_12 = 0
G_21 = 0
G_22 = 0
G_31 = 0
G_32 = 0
G_41 = 0
G_42 = 0
G_51 = 0
G_52 = 0
G_61 = 0
G_62 = 0
Pos_erreicht = 0
I_langsam = 1
Stopwinkel = 40
Winkel_g1_soll = 0
Winkel_g2_soll = 0
Winkel_g3_soll = 0
Winkel_g4_soll = 0
Winkel_g5_soll = 0
Winkel_g6_soll = 0
Start Adc
'--------------- Winkel_g5_soll ----------------
B = Waitkey(#4)
C = Waitkey(#4)
D = Waitkey(#4)
E = Waitkey(#4)
B = B - 48
Winkel_g5_soll = 1000 * B
C = C - 48
C = C * 100
Winkel_g5_soll = Winkel_g5_soll + C
D = D - 48
D = D * 10
Winkel_g5_soll = Winkel_g5_soll + D
E = E - 48
Winkel_g5_soll = Winkel_g5_soll + E
Winkel_g1_ist = 0
Winkel_g2_ist = 0
Winkel_g3_ist = 0
Winkel_g4_ist = 0
Winkel_g5_ist = Getadc(0)
Winkel_g6_ist = 0
Print #4 , "Winkel_g5_soll :" ; Winkel_g5_soll
Print #4 , "Winkel_g5_ist :" ; Winkel_g5_ist
'------------- Fahren --------------------------
While Pos_erreicht < 1
I_langsam = I_langsam + 1
'-------------------- G1 plattform
If Winkel_g1_soll < Winkel_g1_ist Then
G_11 = 1
Elseif Winkel_g1_soll > Winkel_g1_ist Then
G_12 = 1
Else
G_11 = 1
G_12 = 1
End If
'-------------------- G2 schulter
If Winkel_g2_soll < Winkel_g2_ist Then
G_21 = 1
Elseif Winkel_g2_soll > Winkel_g2_ist Then
G_22 = 1
Else
G_21 = 1
G_22 = 1
End If
'-------------------- G3
If Winkel_g1_soll < Winkel_g1_ist Then
G_31 = 1
Elseif Winkel_g3_soll > Winkel_g3_ist Then
G_32 = 1
Else
G_31 = 1
G_32 = 1
End If
'-------------------- G4
If Winkel_g4_soll < Winkel_g4_ist Then
G_41 = 1
Elseif Winkel_g4_soll > Winkel_g4_ist Then
G_42 = 1
Else
G_41 = 1
G_42 = 1
End If
'-------------------- G5 handgelenk
If Winkel_g5_soll < Winkel_g5_ist Then
G_51 = 1
Winkelvergleich = Winkel_g5_ist - Winkel_g5_soll
If Winkelvergleich > Stopwinkel Then
Hand_drehen_rechts
Else
If I_langsam = 2 Then
Hand_drehen_rechts
Else
Hand_drehen_stop
End If
End If
Elseif Winkel_g5_soll > Winkel_g5_ist Then
G_52 = 1
Winkelvergleich = Winkel_g5_soll - Winkel_g5_ist
If Winkelvergleich > Stopwinkel Then
Hand_drehen_links
Else
If I_langsam = 2 Then
Hand_drehen_links
Else
Hand_drehen_stop
End If
End If
Else
G_51 = 1
G_52 = 1
End If
'-------------------- G6 greifer
If Winkel_g6_soll < Winkel_g6_ist Then
G_61 = 1
Elseif Winkel_g6_soll > Winkel_g6_ist Then
G_62 = 1
Else
G_61 = 1
G_62 = 1
End If
Pos_erreicht = G_11 * G_12 : Pos_erreicht = Pos_erreicht * G_21 : Pos_erreicht = Pos_erreicht * G_22
Pos_erreicht = Pos_erreicht * G_31 : Pos_erreicht = Pos_erreicht * G_32 : Pos_erreicht = Pos_erreicht * G_41
Pos_erreicht = Pos_erreicht * G_42 : Pos_erreicht = Pos_erreicht * G_51 : Pos_erreicht = Pos_erreicht * G_52
Pos_erreicht = Pos_erreicht * G_61 : Pos_erreicht = Pos_erreicht * G_62
Winkel_g1_ist = 0
Winkel_g2_ist = 0
Winkel_g3_ist = 0
Winkel_g4_ist = 0
Winkel_g5_ist = Getadc(0)
Winkel_g6_ist = 0
Print #4 , "Winkel_g5_ist :" ; Winkel_g5_ist
Print #4 , "Winkelvergleich :" ; Winkelvergleich
Print #4 , "I_langsam" ; I_langsam
If I_langsam = 6 Then
I_langsam = 1
End If
Wend
Greifer_stop
Hand_drehen_stop
Gelenk_4_stop
Gelenk_3_stop
'Gelenk_2_stop
'Gelenk_1_stop
Waitms 400
Winkel_g5_ist = Getadc(0)
Print #4 , "Winkel_g5_ist :" ; Winkel_g5_ist
End Sub
'###################### 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()
M_5_v = 1
M_5_e = 1
M_5_r = 0
Led = 1
End Sub
Sub Hand_drehen_rechts()
M_5_v = 0
M_5_e = 1
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
'-----------gelenk4---------
Sub Gelenk_4_hoch()
M_4_v = 0
M_4_e = 1
M_4_r = 1
Led = 1
End Sub
Sub Gelenk_4_runter()
M_4_v = 1
M_4_e = 1
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
geht eigentlich nur um die Sub "gehe position, das is das einzige was ich geändert hab..
habs grad nochmal aufgespeilt,... es ging wieder 3 mal gut.. und beim 4. mal kam das geblinke..
Lesezeichen