.: Vinculum :. - Hexabot

[HannoHupmann]

Alles auf die Waage gepackt und gewogen. Jetzt komm ich auf 2,5kg also rund 0,3kg leichter als bei der ersten Messung. Gut es ist noch nicht alles zusammen gebaut aber ich hab auch alle Schrauben und Muttern mit gewogen.

Berechnung:
2,5kg aufgeteilt auf 4 Beine die immer am Boden sind ergibt 0,6kg (=6N) pro Bein an Gewicht. Der längste Hebelarm beträgt 16cm (Bein voll ausgestreckt) ergibt 16cm * 6N = 96Ncm. Die Servos können 14,2kgcm also rund 142Ncm. Also eine Sicherheit von 1,47. Natürlich wird noch das eine oder andere angebaut werden an den Hexa und damit wird das ganze schwerer werden. Bei einer Sicherheit von 1,2 würde ich also ein Gewicht von 2,95kg nicht überschreiten dürfen.
Alternativ gibt es aber noch die Möglichkeit Teile leichter zu machen oder gar durch CFK zu ersetzen.


@movido warum glaubst du ich brauch noch mehr Kraft in den Servos? Ich fürchte eher, dass die Massenträgheit der Beine zu groß ist.

[movido]

@HannoHupmann Naja, probier's erst einmal so. Die Trägheit der Beine ist fast egal. Aber bei einem Hexa wirst Du ganz schnell nur noch drei Beine am Boden haben wollen. Und Du hast ja nicht nur statische Belastung, sondern auch dynamische Kräfte. Wenn Du z.B. einen Schritt machst, dann bewegt sich ja der Körper auch auf und ab, wird also vertikal beschleunigt und abgebremst. Das geht natürlich alles auf die Hebel.

Aber wie gesagt. Schau erst mal, wie's klappt. Mit der aktuellen Konfiguration wirst Du schon sehr weit kommen. Für die Schrittfolge wäre eine gute Inverse Kinematik prima, damit die vertikale Bewegung gering bleibt.

Ich drücke die Daumen und danke ganz herzliche für die tolle Beschreibung!

- Matthias

[HannoHupmann]

So kleines Update, nachdem ich am Wochenende fleißig war und alle Beine wieder zusammengeschraubt habe, sieht der Hexa jetzt wieder gut aus:


Neu hinzugekommen sind die Abdeckungen für die LED-Streifen (diese milchigen Streifen auf den Oberschenkeln). Die Kabel im Inneren des Roboters müssen noch sauber verlegt werden, was ich aber erst machen werde, wenn die zweite Platine fertig ist.

Hier noch eine kleine Detailaufnahme von den beiden Beinen:


Gewogen habe ich den Aufbau noch nicht, das werde ich vermutlich heute oder morgen machen um zu sehen ob es bei den 2,3kg bleibt. Die neuen Servos hören sich sehr viel "leiser" an und sind auch kräftiger, allerdings auch das habe ich noch nicht getestet.

Gestern ist mir noch ein klassischer "Initalisierungs-Bug" aufgefallen, den einige von euch vielleicht auch schon erlebt haben. Die Beine bekommen im Moment alle 90° als Vorgabe - also Mittenstellung. Wird dann die Spannungsversorgung angeschaltet gibt es ein wildes, nicht endenden Zucken und Zappeln. Der Strom geht dabei seh stark hoch. Initialisiert man hingegen nur 4 Beine und hängt die beiden übrigen erst dann an, ist alles ruhig und schön brav und der Ruhestrom beträgt etwa 0,5A

Erst dachte ich, dass mein Netzteil nicht in der Lage ist den hohen Einschaltstrom bereit zu stellen, aber auch mit dem Akku war es nicht möglich alle Servos gleichzeitig zu initialisieren. Entweder muss ich mir in der Schaltung oder in der Programmierung noch etwas überlegen um dieses Anfangsgezappel weg zu bekommen.

PS: Im Fehlschlag der Woche hatte ich geschrieben, dass mir mein Spannungsregler für die 12V Step-Up verschmort ist, aber es war dann doch nur eine kleine 100µH Spule die sich verabschiedet hat.

[HannoHupmann]

Der Roboter wiegt nun 2,9kg, gerade nochmal nachgewogen. Ist also ein bischen schwerer geworden, das wundert mich bei den Servos aber nicht. Allerdings trau ich meiner Personenwaage auch nicht ganz über den Weg, die ist nicht kalibriert und mag schon mal messen was sie mag. Ich muss das Ding mal in der Arbeit auf die Digitalwaage stellen.


Heute habe ich den ersten Programmschnipsel für die Beine geschrieben:
Servo - Speed - Control

Da ich mit einem Propellerchip arbeite kann man wunderbar parallele Prozesse laufen lassen. Im nachfolgenden Code ist zu sehen, dass ich einen "Prozessorkern" (Cog) dafür abstelle die Position an den Servo zu übergeben. Dazu greift er kontinuierlich auf die Variable pos zu. D.h. immer wenn sich im Hauptprogramm der Wert von pos ändert, wird dieser Wert automatisch auch an den Servo übertragen. Ganz ohne lästiges übergeben.

CON 
  _CLKMODE = XTAL1 + PLL16X                             'Set to ext crystal, 16x PLL, 80MHz Clock 
  _XINFREQ = 5_000_000                                  'Frequency on XIN pin is 5 MHz 
  
VAR
  LONG high_pulse, low_pulse, periode, speed, pos, vorzeichen, posinit, lastpos, zielpos
  long stack_c2[55]                                     ' Speicher für den Cog                        

PUB main
  dira[26]~~                                            ' Ausgang für LED - Grün 
  zielpos := 1990                                       ' Zielposition  Servo
  posinit := 950                                        ' Startposition Servo
  speed := 1000                                         ' Geschwindigkeit 0 = schnell, 500 = mittel, 5000 = langsam   
  
'-------------------------------------------------------------------------------------------------------------------------
'### INITIALPOSITION ANFAHREN ###
 pos := posinit
  cognew(moveit, @stack_c2)                             ' Ab hier arbeitet ein Cog kontinuierlich das moveit programm ab                        
  waitcnt(80_000_000 + cnt)                             ' Anfahren der Initalposition
    
  repeat 10
    busy_led                                            ' Bestätigen, dass bereit 
   
'-------------------------------------------------------------------------------------------------------------------------
'### SERVO BEWERGN ###  
  lastpos := posinit
  pos := lastpos
  pos #>= 900                                           ' Untere Servogrenze
  pos <#= 2000                                          ' Obere Servogrenze, wenn die Mathe mal versagt 
  if zielpos > lastpos                                  ' Servo-bewegung vorwärts
    vorzeichen := +1
  if zielpos < lastpos                                  ' Servo-bewegung rückwärts
    vorzeichen := -1

  repeat                                                ' Bewegung                        
    pos += vorzeichen                                   ' Positionsänderung 
    repeat speed                                        ' Warten 
  until pos == zielpos                                  ' Bis zum Ziel und Schluss                              
  
'-------------------------------------------------------------------------------------------------------------------------
'### PROGRAMM ENDE ###
  repeat
    ready_led
  
PRI moveit                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[15]~~                                             ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                 ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * pos)            ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[15]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[15]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)
    
    
PUB busy_led                                            
  !outa[26]                                              ' Ausgang invertieren 
  waitcnt(3_000_000 + cnt)                               ' 300ms warten 
 
PUB ready_led
  outa[26]~~                                             ' LED ON

Wenn man sich mal reingedacht hat ist die Programmiersprache ganz simpel.

Als nächstes wird dieser Code nun so erweitert, dass ich ein Bein steuern kann, also 3 Servos mit beliebiger pos und beliebigem speed. Der Programmteil ist wichtig, weil ich den Hexa nicht immer bei voller Geschwindigkeit laufen lassen will, da schwingt alles zu sehr.

[HannoHupmann]

So Code erweitert auf 3 Servos damit kann ich dann schon ein Bein steuern mit entsprechender Geschwindkeitsregelung. Irgendwas sagt mir aber langsam schon, dass es diesmal nicht bei 800 Zeilen Code bleiben wird...

[HannoHupmann]

So das Programm für alle Beine mit Geschwindigkeitsregelung funktioniert endlich. Das war allerdings nicht so leicht, da ich mit verschiedenen "Bugs" zu kämpfen hatte, die natürlich dazu geführt haben, dass die Servos immer mal wieder absolut nicht das taten was sie sollten oder nur teilweise. Was dann am Ende daran lag, dass ich den 6 Kernen für die Beine etwas zu wenig Memory zugeteilt hatte und sie sich daher ganz gerne gegenseitig überschrieben haben.

Das waren die Softwarebugs, dann hatte ich noch verschiedene Hardwarebugs. Wenn alle 6 Beine angeschlossen und angesteuert werden gibt es den bei Hexas bekannten "epileptischen Anfall", bei dem alle Servos wild in der Gegend herumzucken und Position anfahren die absolut verboten sind. Dieses Verhalten lässt sich auf eine ungeeignete Stromversorgung zurück führen, denn gerade beim Initalisieren verlangen die Servos sehr viel Strom und das kann ein Labornetzteil schon mal in die Knie zwingen.

Daher hier nun ein Bild meiner modifizierten, primären Stromversorgung:


Ist gerade ausgebaut weil ich noch andere Probleme damit hatte die nun aber hoffentlich auch erledigt sind.

[5Volt-Junkie]

Hallo Hanno,

das Teil kann ja bald laufen
Werden alle Beine von einem Propeller angesteuert?

Ich sehe gerade dass die Programmiersprache ziemlich nett aussieht und mehrere Cogs reizen mich auch ein bisschen...

[HannoHupmann]

Ja und nein :P

Der Propellerchip enthält 8 sogenannte Cogs, das sind 8 physikalisch getrennte Kerne die sich den gemeinsamen Speicher teilen. Jeder Cog kann zu jeder Zeit auf den Speicher zugreifen und nacheinander in den Speicher schreiben. Der Charm dieses Chips für einen Hexa ist natürlich, dass man den Cogs eins und zwei die Bahnkurve berechnen lässt, wärend die Cogs 3 bis 8 die 6 Beine kontinuierlich mit Servo-PWM-Signalen versorgen. Die Bein-Cogs greifen dabei einfach immer auf die Variable Xpos zu und wandeln diese in Servosignale (Siehe die Programme mit dem Suffix PRI).
Im unten stehenden Programm arbeitet das Main Programm mit Winkeln in Grad und erst in den Cogs wird daraus ein PWM Wert erzeugt, das macht es leichter den Überblick zu behalten, da sich jetzt alle Beine gleich bewegen, egal wie rum die Servos eingebaut sind.

CON 
  _CLKMODE = XTAL1 + PLL16X                             'Set to ext crystal, 16x PLL, 80MHz Clock 
  _XINFREQ = 5_000_000                                      'Frequency on XIN pin is 5 MHz 
  
Var
  'LONG high_pulse, low_pulse, periode, speed 
  LONG speed
  LONG Apos[3], Aini[3], Alast[3], Aziel[3], Asign[3] 
  LONG Bpos[3], Bini[3], Blast[3], Bziel[3], Bsign[3]  
  LONG Cpos[3], Cini[3], Clast[3], Cziel[3], Csign[3]
  LONG Dpos[3], Dini[3], Dlast[3], Dziel[3], Dsign[3]
  LONG Epos[3], Eini[3], Elast[3], Eziel[3], Esign[3]  
  LONG Fpos[3], Fini[3], Flast[3], Fziel[3], Fsign[3]  
  BYTE x
  long stack_a[15], stack_b[15], stack_c[15], stack_d[15], stack_e[15], stack_f[15]    ' Speicher für den Cog                        
pub main

  init
  Aziel[0] := 45                                      ' Zielposition  Servo
  Aziel[1] := 90
  Aziel[2] := 90
  
  Bziel[0] := 45                                       ' Zielposition  Servo
  Bziel[1] := 90
  Bziel[2] := 90
  
  Cziel[0] := 45                                       ' Zielposition  Servo
  Cziel[1] := 90
  Cziel[2] := 90
  
  Dziel[0] := 45                                       ' Zielposition  Servo
  Dziel[1] := 90
  Dziel[2] := 90

  Eziel[0] := 45                                       ' Zielposition  Servo
  Eziel[1] := 90
  Eziel[2] := 90

  Fziel[0] := 45                                       ' Zielposition  Servo
  Fziel[1] := 90
  Fziel[2] := 90
  
  speed := 5000 
'-------------------------------------------------------------------------------------------------------------------------
'### INITIALPOSITION ANFAHREN ###
  x:=0
  repeat 3
    Apos[x] := Aini[x] 
    Bpos[x] := Bini[x] 
    Cpos[x] := Cini[x] 
    Dpos[x] := Dini[x]
    Epos[x] := Eini[x] 
    Fpos[x] := Fini[x]
    x++

  coginit(3, moveA, @stack_A)
  coginit(4, moveB, @stack_B)                              
  coginit(5, moveC, @stack_C)                              
  coginit(6, moveD, @stack_D)              ' Ab hier arbeitet ein Cog kontinuierlich das moveit programm ab
  coginit(7, moveE, @stack_E) 
  coginit(8, moveF, @stack_F) 
  waitcnt(100_000_000 + cnt)             ' Anfahren der Initalposition
    
  repeat 10
    busy_led                                          ' Bestätigen, dass bereit 
   
'-------------------------------------------------------------------------------------------------------------------------
'### SERVO BEWERGN ###  
  x:=0
  repeat 3
    Alast[x] := Aini[x]
    Apos[x]  := Alast[x]
    Apos[x] #>= 20                                           ' Untere Servogrenze
    Apos[x] <#= 160                                         ' Obere Servogrenze
  
    Blast[x] := Bini[x]
    Bpos[x]  := Blast[x]
    Bpos[x] #>= 20                                           
    Bpos[x] <#= 160  

    Clast[x] := Cini[x]
    Cpos[x]  := Clast[x]
    Cpos[x] #>= 20                                           ' Untere Servogrenze
    Cpos[x] <#= 160  

    Dlast[x] := Dini[x]
    Dpos[x]  := Dlast[x]
    Dpos[x] #>= 20                                           ' Untere Servogrenze
    Dpos[x] <#= 160

    Elast[x] := Eini[x]
    Epos[x]  := Elast[x]
    Epos[x] #>= 20                                           ' Untere Servogrenze
    Epos[x] <#= 160                                         ' Obere Servogrenze, wenn die Mathe mal versagt 

    Flast[x] := Fini[x]
    Fpos[x]  := Flast[x]
    Fpos[x] #>= 20                                           
    Fpos[x] <#= 160
    
    if Aziel[x] > Alast[x]                                    ' Servo-bewegung vorwärts
      Asign[x] := +1
    if Aziel[x] < Alast[x]                                    ' Servo-bewegung rückwärts
      Asign[x] := -1
      
    if Bziel[x] > Blast[x]                                    ' Servo-bewegung vorwärts
      Bsign[x] := +1
    if Bziel[x] < Blast[x]                                    ' Servo-bewegung rückwärts
      Bsign[x] := -1
    
    if Cziel[x] > Clast[x]                                    ' Servo-bewegung vorwärts
      Csign[x] := +1
    if Cziel[x] < Clast[x]                                    ' Servo-bewegung rückwärts
      Csign[x] := -1
    
    if Dziel[x] > Dlast[x]                                    ' Servo-bewegung vorwärts
      Dsign[x] := +1
    if Dziel[x] < Dlast[x]                                    ' Servo-bewegung rückwärts
      Dsign[x] := -1

    if Eziel[x] > Elast[x]                                    ' Servo-bewegung vorwärts
      Esign[x] := +1
    if Eziel[x] < Elast[x]                                    ' Servo-bewegung rückwärts
      Esign[x] := -1  

    if Fziel[x] > Flast[x]                                    ' Servo-bewegung vorwärts
      Fsign[x] := +1
    if Fziel[x] < Flast[x]                                    ' Servo-bewegung rückwärts
      Fsign[x] := -1 
    x++

  repeat                                                            ' Bewegung   
    x:= 0                                                                        
    repeat 3
      Apos[x] += Asign[x]                                  ' Positionsänderung
      if Apos[x]==Aziel[x]                                   ' Ziel erreicht
        Asign[x]:=0
      Bpos[x] += Bsign[x]                                    ' Positionsänderung
      if Bpos[x]==Bziel[x]                                   ' Ziel erreicht
        Bsign[x]:=0
      Cpos[x] += Csign[x]                                    ' Positionsänderung
      if Cpos[x]==Cziel[x]                                   ' Ziel erreicht
        Csign[x]:=0
      Dpos[x] += Dsign[x]                                    ' Positionsänderung
      if Dpos[x]==Dziel[x]                                   ' Ziel erreicht
        Dsign[x]:=0
      Epos[x] += Esign[x]                                    ' Positionsänderung
      if Epos[x]==Eziel[x]                                   ' Ziel erreicht
        Esign[x]:=0
      Fpos[x] += Fsign[x]                                    ' Positionsänderung
      if Fpos[x]==Fziel[x]                                   ' Ziel erreicht
        Fsign[x]:=0

      x++
    repeat speed                                             ' Warten
  until ((Asign[0] == 0) AND (Asign[1] == 0) AND (Asign[2] == 0)AND(Bsign[0] == 0) AND (Bsign[1] == 0) AND (Bsign[2] == 0)AND (Csign[0] == 0) AND (Csign[1] == 0) AND (Csign[2] == 0)AND(Dsign[0] == 0) AND (Dsign[1] == 0) AND (Dsign[2] == 0)AND(Esign[0] == 0) AND (Esign[1] == 0) AND (Esign[2] == 0)AND(Fsign[0] == 0) AND (Fsign[1] == 0) AND (Fsign[2] == 0))                                  ' Bis zum Ziel und Schluss                              
  
'-------------------------------------------------------------------------------------------------------------------------
'### PROGRAMM ENDE ###
  repeat
    ready_led
    
pub init
  dira[26]~~                                          ' Ausgang für LED - Grün   
  Aini[0] := 90
  Aini[1] := 90
  Aini[2] := 90
  
  Bini[0] := 90
  Bini[1] := 90
  Bini[2] := 90
  
  Cini[0] := 90
  Cini[1] := 90
  Cini[2] := 90

  Dini[0] := 90
  Dini[1] := 90
  Dini[2] := 90

  Eini[0] := 90
  Eini[1] := 90
  Eini[2] := 90

  Fini[0] := 90
  Fini[1] := 90
  Fini[2] := 90
  
PRI moveA|periode, high_pulse, low_pulse                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[22]~~                                                
  dira[23]~~
  dira[24]~~
                                                ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                
                                                        ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * (Apos[0]* 8 +799 +50 ))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[22]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[22]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (Apos[1]* 8 +795 -70 ))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[23]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[23]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (Apos[2]* 8 +799 +0))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.
    outa[24]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[24]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse) 
  
PRI moveB|periode, high_pulse, low_pulse                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[19]~~                                                
  dira[20]~~
  dira[21]~~
                                                ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                
                                                        ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * (Bpos[0]* 8 +800 -50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[19]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[19]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (2192 - Bpos[1]* 8 +50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[20]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[20]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (2173 - Bpos[2]* 8 +80))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.
    outa[21]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[21]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)    

PRI moveC|periode, high_pulse, low_pulse                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[16]~~                                              ' Servo Pin D[2]    
  dira[17]~~
  dira[18]~~
                                                    ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                
                                                        ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * (Cpos[0]* 8 +800 +50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[16]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[16]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (2160 - Cpos[1]* 8 + 0))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[17]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[17]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (2173 - Cpos[2]* 8 +50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.
    outa[18]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[18]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)    

PRI moveD|periode, high_pulse, low_pulse                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[13]~~
  dira[14]~~
  dira[15]~~
                                                    ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                
                                                        ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * (2179 - Dpos[0]* 8 -150))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[13]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[13]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (Dpos[1]* 8 + 795  - 50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[14]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[14]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (Dpos[2]* 8 + 795  + 10))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.
    outa[15]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[15]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

PRI moveE|periode, high_pulse, low_pulse                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[10]~~                                              ' Servo Pin D[2]    
  dira[11]~~
  dira[12]~~
                                                    ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                
                                                        ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * (2187 - Epos[0]*8 -10))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[10]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[10]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (Epos[1]*8 + 800  - 150))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[11]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[11]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (Epos[2]*8 + 795 - 50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.
    outa[12]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[12]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

PRI moveF|periode, high_pulse, low_pulse                                               ' Paralleler Prozess der dauerhaft läuft 
  dira[7]~~                                              ' Servo Pin D[2]    
  dira[8]~~
  dira[9]~~
                                                    ' Servo Pin D[2]                       
  periode := ((clkfreq/1000000)*20000)                   ' Länge der gesamten Periode berechnen  
  repeat                                                
                                                        ' Tu das bis du gestoppt wirst - für immer 
    high_pulse := ((clkfreq / 1000000) * (2190 - Fpos[0]*8 + 150))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[7]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[7]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (2187 - Fpos[1]*8 - 50))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.   
    outa[8]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[8]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

    high_pulse := ((clkfreq / 1000000) * (2187 - Fpos[2]*8 - 0))        ' Immer wenn sich der Wert in der Variablen pos ändert 
    low_pulse  := periode - high_pulse                   ' wird dieser Wert an den Servo geschickt.
    outa[9]~~                                           ' Ausgabe des High Pegels 
    waitcnt (cnt += high_pulse)   
    outa[9]~                                            ' Ausgabe des Low Pegels 
    waitcnt (cnt += low_pulse)

pub busy_led                                            
  !outa[26]                                              ' Ausgang invertieren 
  waitcnt(3_000_000 + cnt)                               ' 300ms warten 
 
pub ready_led
  outa[26]~~                                             ' LED ON

Mit dem Laufen wird es leider noch dauern, da ich mich noch gar nicht an die Implementierung der Inversen Kinematik gemacht habe oder die Berechnung der Bahnkurven. Der nächste Schritt wird sein ein paar Grundlegende Bewegungen zu implementieren.

@5V-Junki
Die Programmiersprache nennt sich SPIN ist ähnlich aufgebaut wie Python und sehr intutiv zu programmieren (vor allen fallen diese nervigen Klammern: {} und ; weg, wie sie C braucht und sie ist intuitiver als assembler). Wer übrigens Lust hat kann auch im Programm lustig zwischen SPIN und Assembler wechseln. Soll heißen man kann ein paar Programmfunktionen in Assembler schreiben und vom SPIN Programm darauf zugreifen oder umgekehrt.

[HannoHupmann]

Kennt sich jemand mit Ferritkernen zur Entstörung aus und kann abschätzen ob die bei einem Hexa was bringen um die Servosignale zu entstören? Bisher habe ich darauf verzichtet und nur einen eingebaut der die Servo-Batteriespannung entstört (da war er vom Regler vorgegeben). Ich wüsste nun gerne wo so ein Ferritkern wirklich Sinn macht.

[Slowly]

Hmmm... Kannst Du ein grobes Schema posten, das zeigt wie die Stromversorgung in Deinem Bot aussieht? Ich denke das Problem mit dem Zucken wird sich nicht durch Ferritkerne ausbügeln lassen.