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]