In the code yall defined your filter as:
(256*KFilt/200)
What is KFilt?
What is Yaw rate?
In the code it says… k = 1 to 85…k = 1 to 50…k = 1
to
75…what do this #'s mean and why are they used?
sensor1filt var ???
i would appreciate you all answering these
questions. the full source can be found on the white pages (page 2) Auto Balance Gyro Chip. Thank you and let me know if you all have any questions
about
animation.
Hey
I wish i could help you out on this one, but the autobalance software or any or the programming or electronics for our robot isnt my specialty. Maybe if you email Andy Baker he might be able to help you or give you a contact for the engineers that wrote the autobalance code. I’m also guessing that by the time i post this message Andy will have already posted a reply. lol…
Kyle Gilbert
Team 45 TechnoKats Robotics Team
>In the code yall defined your filter as:
>
>sensor1filt = sensor1filt + ((256 + sensor1 - sensor1filt KFilt/200) - (256KFilt/200)
>
>What is KFilt?
KFilt is a constant that must be chosen experimentally to give you the behavior you want. Its range will necessarily be between 0 and 200 because if KFilt is 0, the filtered sensor value will never change, and if it’s 200, the effect is of no filter at all. (If it were to be more than 200, your filtered value would fluctuate wildly whenever the sensor reading changed.)
To understand this, simplify
sensor1filt = sensor1filt + ((256 + sensor1 - sensor1filt)*KFilt/200) - (256*KFilt/200)
and you get
sensor1filt = sensor1filt + ((sensor1 - sensor1filt)*KFilt/200)
(The reason for the extra terms is to avoid dividing a negative number – a well known problem on the BASIC Stamp.) What we see from this simplification is that each time through the loop, a percentage of the difference between the current sensor reading and the previous filtered value is added to the previous filtered value to give you a new filtered value. The effect of this is to “smooth out” the input from the sensor. (That is the purpose of this type of filter.) In my experience, you will probably want to set KFilt to somewhere between 20 and 120 (10-60% filtering).
What is Yaw rate?
Yaw rate is the rate at which a body turns about a vertical axis. In other words, how fast something is turning to the left or right. This is actually different than what we were measuring for the bridge balancing problem we had to tackle this past year. To solve this problem, we had to turn the gyro sensor so that we were measuring the robot’s pitch rate (the rate at which the nose of the robot pitched up and down.) I guess the yaw rate sensor is commonly called that because that is the application for which it is used in the automotive industry.
In the code it says…
k = 1 to 85…k = 1 to 50…k = 1 to 75
…what do this #'s mean and why are they used?
These are timing loops. For example,
for k = 1 to 75
Serout USERCPU, OUTBAUD, ...]
next
simply executes the Serout command 75 times. I believe the TechnoKats had to figure out by trial and error how many times to repeat each loop in order to balance the bridge.
>sensor1filt var ???
Since sensor1 is a byte variable, sensor1filt needs to be at least byte sized.
Hope it helps.
Greg provided an excellent explanation, but I’ll throw in my two cents worth anyway.
One way to get a good look at how the filter works is to use Excel to make a graph. Here’s a step-by-step guide if you need it.
Put the number 128 in the cells A1, B1 and A2. Put the number 23 in cell C1. In Cell B2 put the following equation (without the quotes): “=B1+(A2-B1)*$C$1/200” then copy this equation and paste it for 30 or 40 rows down the B column starting at B3. Ignore the numbers that it calculates for now. The numbers you put in column A represent the Angle Rate Sensor input and the numbers in column B are the filtered signal that’s being used by the code. Try this: put the number 150 in cells A3 through A20, then put the number 120 in cells A21 through the end (however far you copied column B to). Select A1 through the last row in column B (e.g. A1:B40) and hit the chart button. Select a line chart in the chart wizard and keep hitting the next / finish buttons until it draws a chart. If the y axis starts at 0, double click the y axis and change the min and max values to 110 and 160. Change the value in C1 and see how this effects the filtered values.
As for the numbers in the for - next loops, There were indeed established by trial and error. When “Balance Mode” is initiated, the code takes over control of the drive motors (you’ll notice that the code ignores the joystick input within this routine) and starts driving at a steady rate in one direction while monitoring the Angle Rate Sensor. When a threshold value is reached on the sensor (also determined by trial and error) the robot automatically changes direction by sending a fixed value to the drive motors each time through the loop. Then it changes direction again, this time at a slower speed for a different number of loops. Separate code had to be written for the starting up the ramp in forward or reverse because the drive motors react differently in the two directions.
Hope this sheds a little more light. Feel free to contact me if you want more details.
filter example.xls (19 KB)
filter example.xls (19 KB)