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..