.: Vinculum :. - Hexabot

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

[HannoHupmann]

Frage an alle die Mitlesen:

Ich möchte die Bewegung einens Servos mathematisch invertieren.

Folgende Situation ich habe Servos die sich nach oben bewegen wenn der Vorgabewert steigt (max 2170 Ticks) und sich senken, wenn die Vorgabe sinkt (min 810 Ticks). Nun ist es aber durch den Einbau bedingt, dass manche Servos invertiert laufen, d.h. sie sinken bei hohen werten und steigen bei niedrigen. Für die Ansteuerung wäre es aber leichter, wenn sich alle Servos gleich verhalten. Ich müsste also bei den Servos die sich invertiert verhalten im Programm eine Umrechnung implementieren die aus dem Vorgabewert hohe Werte = steigen und niedrige Werte = sinken, entsprechende Ticks generieren:
Servo A = hoch bei 2170 und niedrig bei 810 (Mitte 1490)
Servo B = hoch bei 810 und niedrig bei 2170 (Mitte 1490)

Irgendwie komm ich gerade nicht drauf, wie ich das mathematisch abbilden kann.

[ikarus_177]

Ich hab' bei meinem Roboter die Servos einfach wie folgt kalibriert: per Taster wurden die beiden Endlagen sowie Winkel von +45° und -45° angefahren. Die jeweiligen Pulslängen bei diesen Winkeln (schließlich sind die Winkel ja das Ergebnis der IK) wurden im EEPROM gespeichert. Dann einfach eine Gerade durch diese Punkte legen (wofür ja schon 2 Punkte reichen sollten) - und fertig ist die Kalibrierung...

Schöne Grüße