PropellerChips: spincode zur ansteuerung v.servos & e-mo

[nomad]

hi leute,
fuer den propeller-chip

eine function zum steuern von servos und e-mots
mit folgender formel:
X = ((X * weight-1)+ Xf) / weight)

x = basevalue
xf = endvalue
if x < xf -> rampUp
if x > xf -> rampDown

weight: muss groesser als 1.0 sein
je groesser weights = desto langsamer gehts zur endstellung



Ramper_vers_10.spin

for : programm basics for ramping Servos Up/down *
- from darwinWeights2.c *
- *
* *
================================================== ======================= *
THIS PROGRAMM IS UNDER THE GNU-LICENCE *
================================================== ======================= *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
================================================== ========================*
* Contributions: *
* parallax.com *
* http://www.parallax.com *
* *
* Spieleprogrammierung M. DeLoran *
* page 145 ff. Mathematics *
* Author: John Olsen *
================================================== ========================*

}


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 Xnew ' dynamic Value
LONG Xold ' "
LONG X ' servoValue
LONG Xf ' EndValue
' if on 1.time Xf is greater as X then RampUp
' if on 1.time Xf is smaller as X then RampDown
LONG weight ' dynamics greater smoother


'intermediate results
LONG w1, z1, z2, z3, z4, z5, z6
' value for abort repeat-loop
LONG ab1, endval


' values for Float32-stuff from propNN2.spin
long testerror
long vlong
word vword
byte vbyte



OBJ
text : "vga_text" ' create vgaText-object
f : "Float32Full"
fp : "FloatString"

PUB Main

f.start

text.start(16)

text.str(string(13,"Ramper-Implementations Vers 0.10 VGA Output ProtoBoard",13,13,$C,5,$C,1))

rampUP

waitcnt(96_000_000+cnt) ' wait only for looking the output of UP

rampDown

PUB rampUp

ab1 := 1.0

weight := 30.0
Xold := 90.0
Xnew := Xold
X := 10.0
Xf := 100.0 ' EndPos only for graphic Screen y-pos is up

endval := f.FSub(Xf,ab1)

REPEAT ' originalformula: X := ((X * (weight-1.0)+Xf) / weight)
w1 := f.FSub(weight,1.0) ' w1 := weight-1.0
z1 := f.FMul(X,w1) ' z1 := X * w1
z2 := f.FAdd(z1,Xf) ' z2 := z1 + Xf
z3 := f.FDiv(z2,weight) ' z3 := z2 / weight
X := z3

text.out($0D) ' newline
text.str(fp.FloatToString(X)) ' old without Float32Stuff -> text.dec(X)
text.out($0D) ' new line

' do something
' you can change X, Xf, weight dynamical

IF X > endVal
text.out($0D) ' newline
text.str(string(13,"Its UP and Quit",13,13,$C,5,$C,1))
text.out($0D) ' newline

Xnew := X

QUIT


PUB rampDown
weight := 30.0
X := Xnew
Xf := 10.0

endval := f.FAdd(Xf,ab1)

REPEAT ' originalformula: X := ((X * (weight-1.0)+Xf) / weight)
w1 := f.FSub(weight,1.0) ' w1 := weight-1.0
z1 := f.FMul(X,w1) ' z1 := X * w1
z2 := f.FAdd(z1,Xf) ' z2 := z1 + Xf
z3 := f.FDiv(z2,weight) ' z3 := z2 / weight
X := z3

text.out($0D) ' newline
text.str(fp.FloatToString(X)) ' old without Float32Stuff -> text.dec(X)
text.out($0D) ' new line

' do something
' you can change X, Xf, weight dynamical

IF X < endVal
text.out($0D) ' newline
text.str(string(13,"Its Down and Quit",13,13,$C,5,$C,1))
text.out($0D) ' newline

Xnew := X

QUIT


{------------------------------------------------------------------------------------}



als attachment noch ein bild zum ungefaehren kurven-verlauf
ramping.jpg
das waers
gruss
nomad
[/img]