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

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

Code:

---

    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]