Caclulus on Propeller?
Litefire
Posts: 108
Hey, i'm trying to do some motor control stuff, and wanted to know if anyone has any hints about performing integration on the propeller.
~~Brian
~~Brian
Comments
of step count)?
actually, i'm not sure how to read the signal out from the encoders on the motors (they came with a motor controller), but i do know for a fact that i can read out the speed of the motor at any time.
reading out the encoder position would require time to do, through the motor controller, because it would require 4 tx/rx I2C writes/reads per encoder (i have two motors). so, i'm not quite sure how to proceed.
~~Brian
there are the specs/documentation on the motor controller, if that helps.
~~B
At any point you have the current velocity v. Let us assume at each time unit you can either increase that by 1, decrease it by 1, or leave it alone.
You also maintain for a given v, the rampdown distance (rd) for that v. This is just the sum of the numbers from 1..v.
If multiplies were fast, this would just be v * (v+1) / 2, but you can do better than that.
Just every time you increase v by 1, add the new v to rd. Every time you decrease v by 1, subtract the old v from rd. Now no multiplies
are needed.
Our movement code is now very simple. We want to traverse a distance d as quickly as possible, subject to a max velocity mv and (let us
assume for now, although it is not required) starting from a v=0 (and therefore rd=0).
while (d > 0) { if (rd > d) { // must decrease velocity rd -= v ; v-- ; } else if (v < mv && rd + v < d) { // can increase velocity v++ ; rd += v ; } else { // no change, nothing to do } // emit velocity v for this time interval . . . d -= v ; // update position }max velocity (otherwise you'll never get to the max velocity). Your value of 1 defines, of
course, your maximum acceleration and deceleration.
What i'm trying to do is take the velocity (which i can read from my motor controller) and determine how far my robot has traveled. This seems to be code to ramp up the speed to a max velocity, and keep track of the velocity. My controller does this for me, i give it a speed (anywhere from full reverse to full forward) and it ramps the motors to that final speed automatically. But i need to know when to tell it to change direction, turn, stop, etc., which means i need to know where i am.
That is my issue, i can't read the data from the encoder registers fast enough, but there is no "distance traveled" register either. So all i have going my way is the ability to know each motor's velocity.
I know i can do it the hard way and determine the time it takes to receive the data and do a sample calculation, then find a v and multiply by that time... but i fear that that is terribly inefficient.
thank you so much for helping me out, sorry for the confusion. also, for the future, i'm trying to implement this into my propeller, and through my untrained eyes that looked to be C or some variant. I'd much prefer either SPIN (preferably) or propeller compliant assembler.
~~Brian
When you say "your controller", is this something that's code in the propeller, or is it an external device?
Either way we need to model what it's doing. As long as it is accelerating or decelerating uniformly
from v1 to v2, you can get the distance traveled with just (v1+v2)/2 * timeinterval. (It's linear, so you
can use an average speed for any interval in which the acceleration or deceleration is constant).
i guess it'll be a little bit of math, a bit of trial & error, and that all-important spark of a genius idea to get it to work.
guess i've gotta get to work.
thanks again!
~~B
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Paul Baker
Propeller Applications Engineer
Parallax, Inc.
((V2+V1)/2)*time_interval
This is the area under a trapesium, basically (V2+V1)/2 is the average velocity in that time period so if you multiply it by the time interval it gives the area of a rectange with height equal to the average speed.
Add up these distances to provide the running distance (actually subtract if V2 < V1). The more often you sample f the more accurate the distance will be.
If time_interval is always the same then you can avoid the multiply. For example if it was travelling at 1m/s and the time interval was 0.1 seconds then rather than do a multiply by 0.1 on every addition just multiply the distance you want to travel by 10 to start with. In other words count to 10 rather than 1 if you want to travel 1M.
Graham
Since·the code·was an early·prototype for an educational example,·the application's PID object's integral·code uses the calculus textbook approach of integration by summing up areas...the sum over i of·(delta-Ei multiplied by delta-ti).· Likewise, its derivative code uses (delta-E / delta-t), where delta-E is Ei - Ei-1.· It's kind of a slow and bloated approach, but it gets the PID algorithm point across.· Also, it relied on a much earlier version of the floating point library.
I'll see about digging up an example that uses a constant delta-t approximation.· It's a lot faster·because it·just uses integer math.· It's all in Spin language, I haven't started on an assembly language version yet.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Andy Lindsay
Education Department
Parallax, Inc.
Post Edited (Andy Lindsay (Parallax)) : 1/4/2007 5:11:17 AM GMT
but could you perhaps give a quick overview of how this code works? it isn't commented particularly well, and i'm having a little trouble with it at the moment. also, what is the main goal of the PID in this example? is it steering, or distance control?
~~Brian
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Paul Baker
Propeller Applications Engineer
Parallax, Inc.
also, how do i use it in my code, will it give me the big number? or will it output a bunch of bytes one at a time representing the 4 bytes in the registers? if it's the second one, how do i compile these into a long in spin?
Thanks so much, I'm so stuck.
~~B
~~Brian
http://forums.parallax.com/attachment.php?attachmentid=44043
Both bots are trying to maintain a distance from an object with some inherently noisy (but inexpensive) infrared parts that are being used to take crude distance measurements. Each bot has two of these IR distance sensors and two motors (servos modified for continuous rotation). Each bot is running a control loop that makes a wheel on a given side of the bot turn based on an "error" distance measurement taken on that side of the bot. This error measurement (E) is the difference between the desired distance and the measured distance.
The first bot is just using (P)roportional control, so it's drive adjustment is only proportional to the error between the desired distance and the actual distance. So the noise from the sensors can be seen in its jittery motions. The second bot uses the code attached to yesterday's 9:00 PM post. So, it adjusts the wheel speed and direction by applying (P)roportioinal, (I)ntegral, and (D)erivative calculations to the error measurement.· Its jitter is much improved.
For understanding the code, open "PID Following Propeller Bot.spin" with the Propeller Tool software, and then press the F8 key. That way, you can browse the various objects the PID FollowingPropellerBot object relies on.
{{PID Following Propeller Bot.spin This program and its objects are all under construction. Modified/enhanced versions of these programs will appear in a forthcoming Parallax Educational text. }} CON _xinfreq = 5_000_000 _clkmode = xtal1 | pll16x L = 0 R = 1 VAR long servo[noparse][[/noparse]2] OBJ schematic : "Propeller Boe-Bot Schematic" piezo : "Piezospeaker" ir[noparse][[/noparse]2] : "Ir Detector" s : "Servos" pid[noparse][[/noparse]2] : "PID" PUB init piezo.beep(6, 2637, 2637) ir[noparse][[/noparse]L].init(8, 9) ir[noparse][[/noparse]R].init(3, 2) servo[noparse][[/noparse]0] := 0 servo[noparse][[/noparse]1] := 0 s.start(0, 1, @servo) ' integral ' set clamps ' kp ki kd point offset high low pid[noparse][[/noparse]L].init(-2.1, -12.02, -0.01664, 25.0, 0.0, 5.0, -5.0) pid[noparse][[/noparse]R].init( 2.1, 12.02, 0.01664, 25.0, 0.0, 5.0, -5.0) main PUB main | dist[noparse][[/noparse]2] repeat dist[noparse][[/noparse]L] := ir[noparse][[/noparse]L].Distance dist[noparse][[/noparse]R] := ir[noparse][[/noparse]R].Distance servo[noparse][[/noparse]L] := pid[noparse][[/noparse]L].calculate(dist[noparse][[/noparse]L]) servo[noparse][[/noparse]R] := pid[noparse][[/noparse]R].calculate(dist[noparse][[/noparse]R])The "Ir Detector" object takes the distance measurements. Two copies of this object are declared with the name ir. Each one has to be initialized by calling its init method and passing it the IR LED and IR receiver I/O pin numbers. After that, its Distance method can be called, which reports a distance between 0 (closest) and 40 (too far, out of range).
The "Servos" object is declared, and named s. You can open that object to see the instructions for how to use it. Basically, you can pass it the address of one or more variables, and it watches those variables from its own cog, and updates each servo's speed and direction based on the variable value(s) it's watching. The values range from -1000 (foll speed clockwise) to 1000 (full speed counterclockwise) with 0 being stop. So, the servos object is initialized by setting servo[noparse][[/noparse]0] and servo[noparse][[/noparse]1] to zero, then passing the address of the servo[noparse][[/noparse]0] variable along with the start and end of a contiguous group of I/O pins with servos connected. In this case, servo[noparse][[/noparse]0] controls the servo connected to P0 and servo[noparse][[/noparse]1] controls the servo connected to P1. If the call had instead been s.start(9,10, @servo), the value stored in servo[noparse][[/noparse]0] would have controlled the servo connected to P9, and servo would have controlled the servo connected to P10.
At this point you might want to take a break and follow the link to the primer that Paul posted.
Two "PID" objects were declared, and given the name pid. These have to be initialized with three constants, kp, ki, and kd, along with a set point, offset and integral clamp values. The PID object automatically calculates the error (E), which is the difference in this case between the set point distance (25) and the measured distance (0 to 40).
The PID object's Calculate has an input parameter that receives the measurement, and it returns the result of these calculations:
···· E = set point - input
···· result = (Kp X E) + (Ki X the integral of E) + (kd X the derivative of E)
For the proportional term's contribution (Kp X E) to the values that control the servo motors is·the error measurement (desired - measured) multiplied by +/- 2.1. That's +2.1 for the right pid object, and +2.1 for the left. So the proportional term's contribution to the pid object's output that gets sent to the servo motor is kp X E.
Recall that the integral term is a running total of the errors, or in the case of this object, an approximation of the area under the error curve. This area gets updated with each measurement and then multiplied by ki = +/-12.02. The result becomes the integral term's contribution to the motor drive. In other words, Ki multiplied by the sum over i of (delta-Ei multiplied by delta-ti).
The derivative term's contribution to the wheel drive is +/- 0.01664. So, that value gets multiplied by the difference between the current error and the previous error, divided by the difference in time. So, the derivative term is: kd X (delta-E/delta-t) or kd X ((Ei - Ei-1)/(ti - ti-1)).
Offset happens to be 0 in this case, since the 0 servo speed value is 0. If a given servo is a little off, and maybe stays still at 57, this value could be adjusted to 57. For motors that required a range from 250 to 1250, the offset might be 750. The result of integration can sometime really get out of hand, adding up and exceeding legal wheel drive values. The integral clamp value prevents the running total of the area under the error curve from exceeding a certain value. In the case of the example code, it's +/- 5.
In the main method, disnt[noparse][[/noparse]L] := ir[noparse][[/noparse]L].distance calls ir[noparse][[/noparse]0].distance, and copies the result to the variable dist[noparse][[/noparse]0]. Same deal with dist[noparse][[/noparse][noparse][[/noparse]R] := ir[noparse][[/noparse]R].distance. It calls ir.distance and copies the result to the dist variable.
The PID object's calculate method gets a value passed to it, the "measured" value of dist[noparse][[/noparse]L] or dist[noparse][[/noparse]R]. The object compares the value it receives to the "set point" value to calculate the Error. Then it does the P, I, and D calculations with the values it received when its init method was called. So, when dist[noparse][[/noparse]L] gets passed to pid[noparse][[/noparse]L], it gets compared to set point, and the Error gets run through the PID calculations, and the result of the method call is the latest motor drive value. The result of this method call gets copied to servo[noparse][[/noparse]L]. Since the servo object is watching this variable, it automatically updates the speed accordingly. Same deal for servo[noparse][[/noparse]R] := pid[noparse][[/noparse]R].calculate(dist[noparse][[/noparse]r]).
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Andy Lindsay
Education Department
Parallax, Inc.
Post Edited (Andy Lindsay (Parallax)) : 1/5/2007 2:37:38 AM GMT
The encoder count is a 32 bit value. You read it one byte at a time and put the bytes together to get a long. The capture count means that when you read the first byte, the other three bytes are captured and kept for you to read.
Huh?
PID controls are not used to filter noisy sensor data. That may be an unintentional effect, but it is not their purpose; they're meant to stabilize feedback loops against the effects of lag.
Your IR sensor may very well be noisy, but what is causing most of the jitter is the delay between the sensor reading data and the motor doing something about it.
long~ ' set to 0
repeat 4
read byte
long << 8
long &= byte
i'm assuming i replace "long" with my long variable name and "byte" with my temporary data variable.
when i do that, it says here:
long << 8 (with my variable it reads encoder << 8)
that the variable requires an operator.
hints?
There is no need to initialize lvar since we'll shift out whatever
it started with.