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.
Wenn man sich mal reingedacht hat ist die Programmiersprache ganz simpel.Code: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
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.
Zitieren
Lesezeichen