Caclulus on Propeller?

Litefire

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

rokicki

Hmm, tell us more. I presume you mean numerical integration? Is this to be able to ramp down your motor and end precisely where you want (in terms
of step count)?

Litefire

right, that's exactly what i want to be able to do.

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

Litefire

http://www.robot-electronics.co.uk/htm/md23tech.htm#encoders

there are the specs/documentation on the motor controller, if that helps.

~~B

rokicki

Well, integration in this case, of linear ramps, is just multiplication. But of course you can do better than that.

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
}

rokicki

The code I give also works for different values of 1. Best values of 1 are values that divide the
max velocity (otherwise you'll never get to the max velocity). Your value of 1 defines, of
course, your maximum acceleration and deceleration.

Litefire

Thanks so much for the help. But i'm a little confused.

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

rokicki

Oh, okay, I guess I read what I wanted to read rather than what you wrote. My apologies.

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).

Litefire

my controller is an external device (here's all of the documentation: http://www.robot-electronics.co.uk/htm/md23tech.htm#mode). I hadn't thought about this, nor had i read the specs right. the motors are stepped up uniformly up to the max velocity, i just need to find out that max velocity.

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

John Abshier

The max velocity is the velocity that you commanded. The problem is that this is 0 (reverse) to 127 (stop) to 255 and the time base is not defined in the documentation. Your geartrain and wheel diameter also influence velocity. Note that when you read the first byte of encoder data it captures the count for the other 3 bytes. That means you get an accurate read as of the first byte read. By having a fixed delay between reads of encoders you can get a velocity in encoder ticks per time. That can be translated into meters per second, mph, etc. But the actual effective diameter of you wheels are not what you measure with a ruler. Here is a link for odometery calibration: http://www-personal.engin.umich.edu/~johannb/Papers/umbmark.pdf

Paul Baker

If it's linear, and you know the ramp time and the start and stop times it is a simple matter of computing the area under the triangle.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Paul Baker
Propeller Applications Engineer

Parallax, Inc.

Graham Stabler

If you sample v at a regular and known interval then the distance travelled in each interval will be

((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

Andy Lindsay (Parallax)

An archived application named PID Following Propeller Bot...zip is attached.· It used PID (proportional integral derivative) control to improve upon the Boe-Bot robot's jittery following with IR distance detection.· The example was implemented on a PE Platform prototype on a Boe-Bot chassis.·

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-E_i multiplied by delta-t_i).· Likewise, its derivative code uses (delta-E / delta-t), where delta-E is E_i - E_i-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

Litefire

this is excellent, and spin is better for me because i don't understand a lick of assembly.

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

Here is his write up on the subject: http://forums.parallax.com/showthread.php?p=529609

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Paul Baker
Propeller Applications Engineer

Parallax, Inc.

Litefire

okay, now can someone give me an overview of what happens when i poll the topmost encoder value. it says "Encoder 1 position, 1st byte (highest byte) and capture count when read" what does this mean?

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

Litefire

the documentation is here (same as above): http://www.robot-electronics.co.uk/htm/md23tech.htm#encoders

~~Brian

Andy Lindsay (Parallax)

Litefire said...
...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?

Sorry about the lack of comments.· This code was in progress when the decision was made not to do anything more with this particular robot.· While the·"PID Following Propeller Bot" object uses PID on a distance measurements input to control wheel speed output, the same could be applied to controlling motor speeds with encoders.· Here is a link to a clip that shows the difference between P and PID control with two differential drive robots.

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, K_i multiplied by the sum over i of (delta-E_i multiplied by delta-t_i).

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 ((E_i - E_i-1)/(t_i - t_i-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

John Abshier

"Encoder 1 position, 1st byte (highest byte) and capture count when read"

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.

Litefire

thanks!

Litefire

thanks, last question: how do i take the 4 bytes of encoder data, and turn them into one unified long value?

Bergamot

Andy Lindsay said...
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.

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.

John Abshier

Off the top of my head

long~ ' set to 0
repeat 4
read byte
long << 8
long &= byte

Litefire

John, thanks.

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?

rokicki

The code really should be:

repeat 4
   lvar := (lvar << 8) + readbyte

There is no need to initialize lvar since we'll shift out whatever
it started with.

Litefire

much better, thank you!