Translating Math to Spin
Kirk Fraser
Posts: 364
Math notation is sometimes confusing when translating a two dimensional formula into a single line computer statement. The formula I need a little help interpreting is equation 3 here: http://mathworld.wolfram.com/LogisticEquation.html
Now I know from Fortran there is an order of execution based on the more complex operations first while in Smalltalk it's straight left to right execution. Which does math symbols use and which does Spin use?
In the link, formula 3 looks translatable to a program. But I'm uncertian if the 1+ is added after or before multiplying the terms in parantheses by e to the specified power. Is this correct?
xt := 1 / 1 + (1 / x0 - 1) * e ** (-r * t)
Thank you.
Now I know from Fortran there is an order of execution based on the more complex operations first while in Smalltalk it's straight left to right execution. Which does math symbols use and which does Spin use?
In the link, formula 3 looks translatable to a program. But I'm uncertian if the 1+ is added after or before multiplying the terms in parantheses by e to the specified power. Is this correct?
xt := 1 / 1 + (1 / x0 - 1) * e ** (-r * t)
Thank you.
Comments
John Abshier
From the manual:
Look at table 2-10 on page 145.
In your case, the equation that you posted would incorrectly evaluate since the / operator would be evaluated before the addition operator (possibly among other things). When in doubt, use parenthesis to change the order of evaluation:
Now, here is a couple of things:
1. Spin is integer only, so your equation wouldn't work very well as is.
2. Spin doesn't have e defined by default.
3. Spin doesn't have an pow function (** is multiply high, which is for 64 bit multiplication).
To get around the Spin integer only, there are two alternatives. You can multiply both the numerator and denominator by the same number (say, 1,000,000) and remember that any number that comes out of your equation is 10^6 times larger than it should be.
The second alternative is to use a floating point library. I recommend F32.
Martin, The ATan2 bug spooked be a bit too, but F32 has been around long enough that I think we're seeing the last of the bugs.
My patched version is available in the F32 thread (post #92).
I believe the ATan method is fine.
I still think F32 is still the best floating point object.
{{Debug scurve }} obj fp : "F32" ' floating point functions fstr : "FloatString" ' Convert Float to String dbug : "Serial Terminal" ' pc serial IO con e = 2.71828 _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 ' use 5MHz crystal pub main | x, it fp.start dbug.startRxTx(31, 30, 0, 115200) ' start debug object waitcnt(clkfreq*3+cnt) ' Wait for Debug Startup dbug.Str(string(13,"Debug Start ")) repeat it from 1 to 15 x := scurve(1000, 1.0, it) x := fstr.FloatToString(x) '**************************** ' bug follows on x variable dbug.Str(string(13, x)) '***************************** dbug.Str(string(10)) ' pub scurve(x0, r, t) ' x0 = Initial point as float at time 0.0 ' r = rate -1.0 decelerate to 0.0 flat to +1.0 accelerate ' t = time 0.0 to 10.0 (time or positons as float to complete curve) ' result = % of top speed / 100 ' http://mathworld.wolfram.com/LogisticEquation.html ' http://forums.parallax.com/showthread.php?144766-Translating-Math-to-Spin ' xt := 1 / (1 + ((1 / x0) - 1) * (e ** (-r * t))) x0 := fp.fsub(fp.fdiv(1.0, x0), 1.0) r := fp.exp(fp.fmul(fp.fneg(r), t)) result := fp.fdiv(1.0, fp.fmul(x0, r))Hi Kirk,
Yes, a variable that will be used as a float should be setup as a long.
You can't mix floats and integers in math functions. If you've going to perform a float calculation on a number it needs to be a floating point number before using it as a parameter in a float method call.
Hopefully this version will clear up some of these issues.
{{Debug scurve}} CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 ' use 5MHz crystal 'e = 2.71828 BAUD = 115200 VAR long fSCurveX long strPtr OBJ fp : "F32" 'floating point functions fstr : "FloatString" 'Convert Float to String dbug : "Parallax Serial Terminal" ' pc serial IO PUB Main | x, it fp.start dbug.start(BAUD) ' start debug object waitcnt(clkfreq * 3 + cnt) ' Wait for Debug Startup dbug.Str(string(13,"Debug Start ")) repeat it from 1 to 15 fSCurveX := SCurve(1000.0, 1.0, it) strPtr := fstr.FloatToString(fSCurveX) dbug.Char(13) dbug.Str(strPtr) 'dbug.Char(10) ' Not needed with PST repeat ' keep cog alive PUB SCurve(x0, r, t) ' x0 = Initial point as float at time 0.0 ' r = rate -1.0 decelerate to 0.0 flat to +1.0 accelerate ' t = time 0.0 to 10.0 (time or positons as float to complete curve) ' result = % of top speed / 100 ' http://mathworld.wolfram.com/LogisticEquation.html ' http://forums.parallax.com/showthread.php?144766-Translating-Math-to-Spin ' xt := 1 / (1 + ((1 / x0) - 1) * (e ** (-r * t))) t := fp.FFloat(t) x0 := fp.fsub(fp.fdiv(1.0, x0), 1.0) r := fp.exp(fp.fmul(fp.fneg(r), t)) result := fp.fdiv(1.0, fp.fmul(x0, r))I haven't check through the math involved. I just made integers floating point numbers so the equation would have a chance to work.
You can also see how to display a float using the float to string object.
This is the output of the above code. Is it anything like you were expecting?
Edit(3/11/15): Warning, the code attached is an old version. There are likely better options available.
I plan to upload this program or an improved version to my GitHub account
If there isn't code similar to what is attached here on my on GitHub, send me a message and I'll make and check for any improved versions of the code.
Hi Duane,
Thanks. I copied your code but the best I can get is the first statement "Debug Start" without the numbers. What would you suggest?
On the result numbers, they don't fit the curve shown in the link
http://mathworld.wolfram.com/LogisticEquation.html
Is there some kind of math error? I traced the math given in the link to the code and it looks ok to me. But it should be delivering numbers that go from 0 to 1 which would allow various uses such as slowly turning on an LED. Changing the x parameter should produce 1 to 0. Am I starting with X0 = 1000.0 incorrectly?
I want to use the formula to generate a series of numbers, 0 to 1, 1, 1 to 0 as X moves within the range less than your position sensor output smaller than 0 to 4095 such as 1000 to 2000, then in reverse over the range 2000 to 1000 varying the output again from 0 to 1, 1, 1 to 0 producing a smoothed trapezoid curve in each direction which will reduce jerk and backlash motion in a robot or CNC.
Did you try the Spin file attached to post #8?
I think your code misses the 1+ from the formula: xt := 1 / (1 + ((1 / x0) - 1) * (e ** (-r * t)))
the last line should be:
Andy
Andy,
With your correction, the output is:
Which looks better than the earlier output but I don't think this is what Kirk is looking for either.
Thanks for pointing out the error in the code.
And t should start at 0 not 1.
So a main-loop can be: (the first parameter of SCurve can be varied between 0.05 and 0.95 to get the different colored curves)
repeat it from 0 to 15 fSCurveX := SCurve(0.1, 1.0, it) strPtr := fstr.FloatToString(fSCurveX)Andy
Thanks for looking the math over to see what's it's supposed to do. I hadn't looked at the math behind the equations yet myself.
I'm pretty sure Kirk wants to use these equations to smooth out the movement of some actuators (I believe they're hydraulic). I myself wonder if you need such complicated equations to get nice smooth ramping.
If I use an acceleration of one unit distance per time unit squared and a max speed of 25 distance units per time unit I get a displacement graph like this:
This graph also shows a deceleration of the same magnitude as the acceleration. The shape of the curve could be squashed or stretched by changing the acceleration magnitude.
I personally think this would be easier to program and use parameters which would be easier to understand.
What do you think Kirk? Andy? Others?
Here is a test with output to PropTerminal:
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 OBJ term : "pc_interface" PUB Main : y | t term.start(31,30) repeat t from 0 to 300 y := smooth(50,200,300,t) term.plot(t,240-y) PUB smooth(x0,x1,steps,n) : x | h h := (x0+x1) / 2 x := cos(n*4096/steps, x0-h ) + h PUB cos(angle, range) : s | c,z angle += 2048 c := angle & $800 z := angle & $1000 if c angle := -angle angle |= $E000>>1 angle <<= 1 s := word[angle] if z s := -s return (s*range)~>16 ' return cosine = -range..+rangeAnd the output:Andy
Cool! Very nice.
I'm pretty sure the curves are identical in shape (any difference could be removed by a squash or stretch). This reminds me of the sinusoidal motion demo in physics where a ball oscillating on a spring has the same vertical motion as a ball on the edge of a rotating disc.
I'm still thinking an acceleration algorithm may be a better fit for this application since I'm pretty sure Kirk will want to compute both the displacement and the speed.
I'll modify your code to illustrate the way I'm thinking it could be done. I'll post the code and PropTerminal output in a bit.
Thanks for the example of how to use the sine ROM table. I haven't used the ROM tables myself.
While I think the curve produce by using the ROM sine looks nice, I think in reality there will likely be a section of the motion path without acceleration.
It took a bit of fiddling to get integer values for acceleration, max speed and distance traveled that would fill the PropTerminal display.
This the code I ended up using (some comes from the stepper motor code I'm working on).
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 MAX_SPEED = 200 ACCELERATION = 2 DISTANCE_TO_MOVE = 40_000 MAX_DISPLAY_Y = 220 DISPLAY_SCALE = DISTANCE_TO_MOVE / MAX_DISPLAY_Y ' driverState enumeration #0, RAMP_UP, FULL_SPEED, RAMP_DOWN, STOP VAR long speed, time, distance long rampDownTarget byte driverState OBJ term : "pc_interface" PUB Main term.start(31,30) driverState := RAMP_UP speed := 0 repeat Move term.plot(time++, MAX_DISPLAY_Y - (distance / DISPLAY_SCALE)) while driverState <> STOP repeat PUB Move case driverState RAMP_UP: RampUp FULL_SPEED: FullSpeed RAMP_DOWN: RampDown distance += speed PUB RampUp result := speed + ACCELERATION if result => MAX_SPEED speed := MAX_SPEED rampDownTarget := DISTANCE_TO_MOVE - distance if rampDownTarget =< 0 driverState := RAMP_DOWN else driverState := FULL_SPEED else speed := result PUB FullSpeed if distance + speed => rampDownTarget driverState := RAMP_DOWN PUB RampDown result := speed - ACCELERATION result #>= 1 if distance + result => DISTANCE_TO_MOVE speed := DISTANCE_TO_MOVE - distance driverState := STOP else speed := resultThis is the output to the PropTerminal.
I think the main difference in this curve from the one produced using the sine tables is the straight line section of constant speed. If this constant speed section were removed, I think the graphs would look very similar.
My previous code segment was just attempting a third of the problem, the acceleration or rise from 0 to 1. The second stage must stay at 1 for most of the move along an axis. The third stage decelerates from 1 to 0 so the motion along say the X axis will arrive exactly at the specified point. So the result is a trapezoid but I'm trying for smoother curved transitions instead of the angles of a trapezoid.
At this stage of my understanding and project any curve will be an improvement but eventually Duane is right I will want to be able to specify acceleration for a controlled jerk into action. I'm guessing some tweaking will be needed based on weight of the object moved. Can a weight parameter be used to calculate the appropriate curve squashing?
In the long run I'll need to use a Proportional Integral Derivative (PID) algorithm or other feedback loop to hold a robot or CNC motion at the position specified as end of motion, until a new motion command is generated, to counteract gravity, backlash, and other forces which might act on the part being moved. I currently don't know how a PID could be integrated into the trapezoid like curve since it's intended to swing back and forth.
I don't know enough about PID's, if they are used in the whole motion or just at the end of travel. Perhaps there is some simpler way to hold position until a new position command is received. In any case getting it to work at all would be progress. I wasn't able to get numbers out of my setup let alone a graph. How do you guys do that? Thank you.
By using acceleration, the movement should be smooth. You used the work "jerk" which has a technical meaning as well as the usual one most people think about when they hear/use the word. Velocity is the change of position over time, acceleration is the change in velocity over time and the jerk is the change in acceleration over time. In general a constant acceleration is enough for smooth motion.
I'm guessing your use of the word "jerk" referred to an abrupt movement?
Generally a PID algorithm would take care of the weight. For example, my Mecanum wheeled robot requires more power to drive on carpet than a smooth floor. When the robot encounters carpet, the control algorithm automatically compensates for the increased resistance which allows the robot to maintain the desired speed.
The target speed would be calculated by using whatever curved trapezoid algorithm you use for computed desired speed. The PID algorithm would use feedback in order to make the actual measured speed match this target speed. They can remain independent of each other.
The PID algorithm would likely be active the entire path of the mechanism.
For one thing I chose my constants in order to avoid the need for fractional numbers. I then pretty much just copied Andy's method of graphing the output. Andy used a program called PropTerminal to graph the output. Here's a thread with a link and a bit of discussion.
@Andy, Are you the same Andy who wrote PropTerminal? If so, very, very cool!
Yes, I am. Thank's for the kudos. Here is the original thread about PropTerminal. This was my first post in this forum, it's quite a few years since...
Andy
Yes although there can be many reasons for needing an abrupt movement. Say you want to make a robot move through a spring loaded door by pushing it open with just enough force so it shuts behind the robot instead of on the robot. In my human experience that takes precise acceleration control. Or say the hydraulic cylinders encounter "stiction" which is a technical term for a tendency of staying in position even when force is applied due to O-rings sticking to the wall of the cylinder. A measured amount of acceleration might overcome a tendency to stick well enough to appear that they're smooth.
Great! I'm glad you know this stuff.
Ok, but I'm not even able to get the numbers as you might see when you load up my software. Maybe it's my copy of the serial terminal? Or maybe I don't know how to do a reset from the new Propeller Project Board which doesn't have a reset switch, or maybe I have some setting wrong so the terminal interrupts the Propeller Tool and vice versa instead of operating as one like the old Basic Stamp software. Thanks, I'll try the PropTerminal and see if that fixes my problem.
My Propeller Project board has reset button. It's near the corner farthest away from the USB connector. The button faces forward rather than up. I think you could also reset the Prop from within PropTerminal.
I'm attaching spin code which doesn't require you to reset the Propeller to see the graph. Just load "AccelerationSCurve130716a.spin" into the Prop open PropTerminal, Select Options\Com Enabled (to connect to the serial line) from the menu. You should see a bunch of "Press any key to start." messages. When a key is pressed on the keyboard, the screen will be cleared and the graph displayed.
I've noticed you use a different serial object than what I'm used to. Is there something you don't like about Parallax Serial Terminal.spin and/or Parallax Serial Terminal.exe.
Andy's PropTerminal is cool in that it will let you do some basic graphics and uses colored text but for reading debug text, I much prefer to use Parallax Serial Terminal.exe. PST allows much more text to be displayed at one time than PropTerminal.
I'm attaching a similar version of the graphing code which displays the output as numbers. The Program "AccelerationPst130716b.spin" is intended to be used with PST. This program also starts with a "Press any key to start." message so you can take as long as you like enabling PST.
This is the output from "AccelerationPst130716b.spin".
The program "AccelerationPst130716b" adds an acceleration value ("a" in the output) to the speed value ("s") with each loop of the program.
You shouldn't need to reset either of these programs to see the output but I thought I'd let you know you can reset the Propeller from PST. Checking and unchecking the DTR box will reset the Propeller.
Here's the box I'm referring to.
Let me know if you have trouble with either of the attached programs.
Edit(3/11/15): Warning, the code attached is an old version. There are likely better options available.
I plan to upload this program or an improved version to my GitHub account
If there isn't code similar to what is attached here on my on GitHub, send me a message and I'll make and check for any improved versions of the code.
The PST I sent is the same as yours except I removed "Parallax" from the file name since the length is too long for the minimized Propeller Tool as it overwrites other tab names when several are in use.
So what is next? PID? Or if you're pretty much done, I'd appreciate you looking at my call to your sensor demo as I don't think I have the right method to get the data which goes to the screen.
I'm working on some similar code that will make it easy to adjust the acceleration and maximum speed parameters.
I've considered adding a magnetic encoder to a threaded rod attached to the stepper on my new CNC machine but I'm not sure if the stepper would behave anywhere near as similar to your hydraulic gear to be a valid test rig. If it is close enough to use for some testing, then I could write the code with my stepper in mind and once I get it working switch the control type from a variable frequency to a PWM duty cycle.
This is what I'm currently working on.
Although your algorithm does what you wrote, it doesn't exactly do what I need. It took working to integrate your methods before I realized they do not produce a smoothed trapezoid but a literal S curve. One obvious thing is the fast center section of the curve ought to be flat at 1.0, not going up at 45 degrees. I was suspicious when I saw your ever increasing d-value numbers but didn't figure it out until now. The s-value numbers look like they can be adapted to make a pin go from 0 to 1 and back.
I left out the displacement since it would distort the scale of the speed curve. This speed curve results from a constant acceleration. As I mentioned previously, I think this would be sufficent for most applications.
If jerk were added to the movement algorithm (unnecessary in my opinion) the speed curve would look something like this.
In this example I used a jerk of 0.05 with a maximum acceleration of 2 (units are not defined in this example, I just choose numbers to illustrate the principles). Again the maximum speed is 200. I didn't do the calculations necessary to get the displacements to be equal in both sets of data.
I imagine this second graph is what you are looking for? I still think a constant acceleration (first graph) would be fine and you don't need the added complication of a variable acceleration.
When you drop an item it undergoes constant acceleration and it ramps up to speed nice and smooth. Even though the first speed graph looks like it has abrupt changes, the displacement of the object undergoes nice smooth movement as illustrated by the displacement curve (post #17).
Edit: I'm sure there are applications which would benefit from the acceleration changing gradually, I don't think they are very common. I'm concerned with how using jerk in a motion algorithm will affect the performance of a PID system. My initial thoughts are it would make the PID system sluggish but I don't have experience to back up this assumption.
Yes, this second graph is what I'm looking for. There are two reasons, one is I'm trying for human like motion and humans can control their motions that precisely. Also, some professional motion control systems which control larger industrial systems than the one I'm building come pre-programmed with the S-curve solution. I believe that is because of the large weights being moved and stabilized. I believe the same behavior can had with an affordable Propeller based solution (amplifying if needed) instead of having to choose the expensive control systems professionals use. For toy robots and other lightweight motion applications where the mass doesn't create much jerk, I'm sure you are correct a simple trapezoid would work fine. My current robot is designed to lift 250 lbs and a future robot may be built to lift 1000 lbs so I do want the software that produces the second graph.
Unfortunately I have no experience with PID or new insights on it at present. The only thing I know I want is to have it maintain its position against all external and internal forces when it has reached its last commanded position and there is no current input command. Any further functionality is welcome.
The more I think about ramping the acceleration and not just ramping the speed, the more I like the idea. I still think a constant acceleration would give smooth movement but it sure seems using a ramped acceleration has advantages.
Here's the displacement curve again with constant acceleration:
With jerk added to the equation, the displacement curve has a gentler transition at the beginning and end.
I don't think it would be very difficult to add jerk to a motion algorithm.
Excellent!