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