.: Vinculum :. - Hexabot

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

[HannoHupmann]

Siehe Beitrag #126 da ist meine "Primäre Versorgung" zu sehen. Von dort geht 3,3V - 5V und GND zum µC und die Signalkabel zu den Servos (in Reihe ein 100Ohm Widerstand). Vor jedem Bein ist eine "Verteilerplatine" gebaut hier werden die Servos mit 6V Spannung versorgt und mit 100nF + 100µF gepuffert. Seit gestern ist diese 6V Schiene mit EMV Geflecht ummantelt und von der Signalleitung getrennt (Signale sind nun unten verlegt und Versorgungsspannung oben).

Zusätzlich habe ich heute noch die längsten Servoleitungen in der Nähe des µC mit Feritkernen versehen (die liegen eh noch rum also kann ich sie auch einbauen).

Das Zucken des Servos E0 kommt allerdings nicht von der Servospannung, Signalleitung oder sonst was, sondern von der kaputten Elektronik im inneren. Er bewegt sich zwar, aber ohne Kraft. Irgendwas ist an dem nicht mehr ganz in Ordnung und daher wurde er jetzt ausgetauscht. Schon funktioniert alles wie gewünscht.