PWM-IN for Tilt Sensors

[Pacetech]

I am trying to read a MEMSIC MXD2020EL dual axis accelerometer. http://www.memsic.com/memsic/data/products/MXD2020EL/MXD2020E_FL.pdf

I see PWM-IN may read the pulse width, but I'm more interested in the duty cycle. The period is about 10ms, so what I really need to measure is the pulse width and the period. Then I can determine the % duty cycle and do the conversion to tilt angle.

I guess I could parallel 2 timers, one to measure the pulse width, and another to measure the period.

[RMDumse]

Have you looked at the ADXL202 example in the Downloads page?

http://www.newmicros.com/isopod/appnotes/adxl202.txt

This uses a statemachine to alternatively use the timer to check high periods, then low periods. While it doesn't usually capture a single period, as long as the accel is stable over a period greater than a few cycles, it will get closely corresponding readings.

[Pacetech]

Here is what I ended up doing and it works great - in case anyone is interested in the code.

http://www.newmicros.com/isopod/appnotes/Memsic_Tilt_Sensor.txt

Edit: Added download link above

[RMDumse]

Yes, this forum has a slight bug, it won't let anyone upload files. We have an upgrade, but we're worried if we load it, we'll loose everyone's information, and all the wealth of past posts. So for now, we just live with it. Pls email to nmitech, which I think you've done before, and he can post it on the site so it is accessible by others. Thanks.

[cwkoehler]

Pacetech,
So did you end up using 2 timers in parallel, one to measure the pulse width, and another to measure the period in your example?
What I need to do is similar except that I am using the ADXL210 instead of the mesmic.
Also, how do I display the calculated angle within the terminal window? Any insight would be greatly appreciated. Im using the IsopoX for this.

thanks
Chris Koehler

[nmitech]

how do I display the calculated angle within the terminal window?

F. ( display the floating point value on stack )
U. ( display the unsigned integer value on stack )

GET-FANGLE F. ( to display the angle in floating point format )
( or )
GET-ANGLE U. ( to display the angle in a whole number )

[Pacetech]

My code should work for you, just need to do the following to adapt it to the ADXL210 .

1. Make sure your time period is always less than 26ms. If not, or if your period is much smaller, you can adjust the prescaler to make it work. But the idea is, for most resolution, set the timer prescaler to be slightly larger than your max period.

2. Modify GET-FANGLE -> this is where the angle calculation is done. You can look at the equation for the Memsic Tilt sensor in Memsic App Note AN-00mx-007.pdf to see what I'm calculating.

The ADXL210 uses a similar equation, instead of dividing by 20% for the Memsic, you need to divide by 12.5% - so just change this line in the code

for memsic:
1 S>F 5 S>F F/ FCONSTANT 20PERCENT EEWORD

for ADXL210
1 S>F 8 S>F F/ FCONSTANT 12.5PERCENT EEWORD

Then replace 20PERCENT with 12.5PERCENT in GET-FANGLE.

[mbailey]

Please excuse how dumb I am but I have an ADXRS321 which has an Analog out, could you help me work out the angles please. Or should I just Buy the PWM version?

I am trying to use an ADXRS300 as well which is a rate sensor, I am just working on that one but probably not right!!!

I am trying to stabalise a camera on the tilt Axis and using the D-rex H-Bridge to drive direct dc motors.

All help would be appreciated

Thanks in advance

Mike

[Dave]

I cannot find a data sheet or information for the part number ADXRS321. Could you please post a pointer to such, or where information about it can be found? Analog Devices appears to have an ADXRSxxx device with an ending of 150, 300, or 401.

If the device has an analog output, it would probably be best to scale the output to the 0 to 3.3 volt range of the ADCs on the Plugapod. You would then use the math available on the Plugapod to change it to an angle measure. Something like :

: GET-DEG ADC0 ANALOGIN 91 / . ;

should take a voltage between 0 and 3.3 volts that would put 0 to 32767 on the stack. 91 is then placed on the stack and divided, to produce a number on the stack between 0 and 360 for a degree measure. The dot . displays that number. The measure and math would need to be changed to reflect the output of the sensor, as well as the desired angle measure (radians, etc)

[Dave]

To be added:

If the rate gyro version was to be used, it appears to output a 2.5 voltage when motionless, with the rate of turn changing the output higher or lower depending on direction. This too could be measured on the ADC pins, but would require different math manipulations to get to degree per second data. It appears the output can range betweeen 0 and 5 volts, with the 2.5 volt center point. This would best be scaled electrically to the 0 to 3.3 volt range of the ADC on the Plugapod, using either a resistor divider, or possibly a precision op-amp.

[mbailey]

the ADXL321 data sheet is here Analog (http://www.analog.com/UploadedFiles/Data_Sheets/950113267ADXL321_0.pdf)

I am thinking of reading this as well as a rate gyro which I have ADXRS300 which as you say is 2.5 in null ADXRS300 (http://www.analog.com/UploadedFiles/Data_Sheets/732884779ADXRS300_b.pdf) for the op amp way, I have used them before but not quite in this respect, what about the resister divider? is that a potential divider? i.e 270 Ohm and 330 Ohm to drop the middle?

I beleive the OP amp would be more accurate, am I correct? could someone help me with that?

When I have converted the gyros into angles how do I then tell the motors to keep it at a given angle? I will be using the H-Bridge and Quad encoders.

Also how could I adjust the sensitivity? I.e. the gain

Sorry to be such a pain to you all.. :o

Thanks Mike

[Dave]

Yes a resistor divider is a potential divider, like a 2.2 K resistor over a 3.3 K. The output of the 5 volts range sensor could go to the top of the 2.2 K witht the signal going to the ADC from between the 2.2K and the 3.3K, with both sides connected to ground on the other end of the 3.3K.

Control of the motors is usually done with PID, that is proportional integral derivative feedback control. This is a somewhat involved discussion that is better taken up in web searches, and books. Essentially you command the motor to a position, that can be determined by some type of feeedback like and encoder or potentiometer, and compare this to the commanded position. The error that has been measured is used in math manipulations (the proportional, integral and derivative above) to calculate a new drive signal each cycle. Various controllers may run through these calculations 20 to 100 times per second to achieve accurate drive. There is an example of PID drive for a differential drive robot (2 motors, like pan and tilt) on the download page called PID controls (http://www.newmicros.com/isopod/appnotes/pid17.txt)

Sensivity could also be adjusted by math manipulations. If there was the need for continuous tweaks, a couple of trimmer pots could be used. Or possibly monitored push buttons, if running low on A/D inputs.

[mbailey]

Ok guys I have the gyro connected now, I have a 1.6K and a 3.3K which by my calcs will make from 5V max 3.36,

ADXRS300
The centre voltage is 1.642 and it ranges from 0.25 to 3.22V.

Now I have this sorted would someone please help me with reading it on an Analog input, I need to translate voltage difference into angles, I think it is 5mV per degree per second, but I'm sure I can confirm that once I am reading the info and in the calibrating stage.

I have an Accelerometer too for later stages so i can confirm the angles, but I'm sure It can be done with a rate gyro.

How do I store the voltage range and time per movement?
Do I need to, or should I just adjust the math according to how it performs with the motors?

Then How do I put the results into the PID you have mentioned?

Hope this makes sense..


Mike

[Dave]

Note that when the output is scaled, the response will be scaled also. So a response of 5 mV per degree per second would then become 3.3 mV per degree per second.

The gyro appears to be a device that measures relative movement. So regardless of it's inital orientation, it should settle to the middle range of 1.642 volts (or 2.5). Once some kind of twist occurs, it would register and output that, but when still again, the value of 1.642 woud be settled on. It seems it could be used to see how something was moved relative to a position it had initially, but is not a direct indicator of orientation itself.

To measure an angle of how much of a turn has happened, periodic readings of the gyro would have to occur. So a repeating loop with reading the sensor at a known interval would be needed. The
EVERY 50000 CYCLES SCHEDULE-RUNS GYRO-V
would provide a way to check the gyro 100 times per second, though it appears the frequency response of the gyro sensor is 40 Hz. Checking the device at 100 Hz might be ok though. What could be set up is some sort of variable that is given an angle value, like a starting point. This value would be updated by the measurement taken 100 times per second, and scaled accordingly using math functions. The measurements would be subtracted from the null value of 1.642 volts, then the result divided by the time period of 0.01 (or multplied by 100, for 100 times per second). The measurement would then be scaled to the 3.3 mV per degree (since the seconds had just been multiplied out), to get a degree measure of change since the last reading. The scale of the ADC was 0 to 32767, which becomes huge, 0 to 3276700 when multiplied by 100. Scaling for voltage measure to degrees should invlove dividing by 303. This calculation could be simplified by taking the measured ADC value, subtracting 16304 (mid point), then dividing by 3 for the change in angle. This measure of change should then be added (either a negative or positive change) to the value variable previously established as a beginning. So GYRO-V could look like

DECIMAL ( in case HEX has been used previously
: GYRO-V ADC6 ANALOGIN 16304 - 3 / ANGLE @ + DUP ANGLE ! . ; EEWORD
( displays the value of ANGLE + motion measured

Of course the variable ANGLE would have to be declared and some value placed in ot earlier in the program, before the variable is used.

VARIABLE ANGLE EEWORD

: INIT
90 ANGLE !
; EEWORD

So together it might look like


VARIABLE ANGLE EEWORD

: INIT
90 ANGLE ! ( storing an initial value of 90 degrees in ANGLE
; EEWORD

: GYRO-V ADC6 ANALOGIN 16304 - 3 / ANGLE @ + ANGLE ! ; EEWORD

INIT ( run to put a value in ANGLE

EVERY 50000 CYCLES SCHEDULE-RUNS GYRO-V


ANGLE could be displayed by typing in ANGLE @ . when needed, or could be put in a different type of loop to see what value is being stored. PCC loops like BEGIN 13 EMIT ANGLE @ . ?TERMINAL UNTIL are also possible. This type of loop prints very quickly and will probably over-run the serial link of the Terminal program. A method of using this loop might be to put in a delay, to display values only every so many cycles, like this :

LOOPINDEX DELAY
100 DELAY END

: DISP-ANGLE BEGIN DELAY COUNT IF 13 EMIT ANGLE @ . THEN ?TERMINAL UNTIL ;


This has the loopindex DELAY count to 100 before displaying a value retrieved from ANGLE with the @ (fetch) command. BEGIN and UNTIL frame the loop, 13 EMIT is an ASCII command to print without a line feed, so that it prints out over what had previously been printed. ?TERMINAL waits for a keypress on the keyboard to end the loop.

Not sure if that would work with no gyro here, but it might be a start, to tweak with actual measurements. The gyro has a temp sensor that might need to be integrated in to provide more accurate readings.

There is additional information on the web regarding using a gyro in combination with an accelerometer and combining the results using a math technique called Kalman filters to get an angle value. I have not attempted using this yet, so might not have much to offer as far as the math of a Kalman filter.

[mbailey]

Yes I have read about the Kalman filter, very interesting.

I have put the code in but the value seems to altinate sporadically from minus to positive large values. I have put my silly scope in paralell with the gyro output and it is definatly staying constant in the null, I have the ground on VSSA and the 5V supply feeds it,

Trying to work out the code, will it accumalate the readings on top of each other? until I ask for the reading? and I suppose after also.

I am off for a few days now so I won't be able to perform any more tests I don't think but if you could try it out with maybe a pot to give the differential voltages?

Thanks again for your help and I'll be back soon....


Mike

[Dave]

I tried out the WORDs above and they do work, perhaps a little too well. What was attempted was a summing of any variation away from a middle value. In running with a potentiometer, the values blew up quickly if the pot was set very far from the 16304 middle value. What it was meant to do was to add the measured change, which shows up as a voltage variance from the 1.642 v (2.5 v) center value, to a starting position. This should show a relative change in position. Consideration for noise from the sensor will probably need to be accounted for. It has a 3.37 mV (5 mV) per degree per second change, which the ADC can resolve to 0.1 mV (3.3 v divided by 32767). It might be better to scale the number delivered by the ADC by 33 or 34 ( for 33.7) rather than the 3 initially tried in the GYRO-V word.

Perhaps there are some further math manipulations that would first of all provide a deadband to avoid noise (if that is a problem), and second of all make the numbers all range between 0 to 360 for degree measure.

[Dave]

Here is a short little word to take a decimal number and bring it to the range of 0 to 359. Should work well with whatever is stored in ANGLE above.

( NUMBER TO DEGREE TRANSLATION
( INPUT A VALUE -32767 TO 32767 DECIMAL
( TO BE TRANSLATED INTO 0-359 DEG

DECIMAL

: N2DEG DUP 0 < IF 360 MOD 360 + . ELSE 360 MOD . THEN ;

[mbailey]

Thanks Dave, you are very helpful...

I am currently away and will try it when I'm back in the office, sounds good though.

How about the noise? How would I write a code to dampen that?

Would you if at all possible put the previous codes into one code so I can see it properly please????????


Thanks again and looking forward to getting back and tryin it.


Mike

[mbailey]

Hi all I'm back at the office, I have read about filtering E.t.c, also I can reduce the gyro to 30Hz or even lower to simplify things but i Wonder if it will be still high enough to keep the system stable...

Any ideas of software filters? Would be great to get a little codeing idea!!!!


Mike

[Dave]

On the Pods Download Page (http://www.newmicros.com/store/product_details/download.html) there is a Analog to Digital Converter (http://www.newmicros.com/isopod/appnotes/ADC.txt) code example that averages ADC readings. Similar math could probably be used to lessen the affect of any noise that might be present in the gyro readings, if any is there.

[mbailey]

Hi Dave, I really can't get this too work mate, please help!!!!!!

Its just giving me speradic readings and I don't understand about ADC filtering earlier!

here's the null readings:
ADC6 ANALOGIN . 16320 OK
ADC6 ANALOGIN . 16256 OK
ADC6 ANALOGIN . 16192 OK
ADC6 ANALOGIN . 16240 OK
ADC6 ANALOGIN . 16216 OK
ADC6 ANALOGIN . 16240 OK
ADC6 ANALOGIN . 16200 OK
ADC6 ANALOGIN . 16264 OK
ADC6 ANALOGIN . 16280 OK
ADC6 ANALOGIN . 16264 OK


Mike

[Dave]

The posted examples goes to the register and control of the Analog pins directly, where ADC6 ANALOGIN . is a simpler method of getting a single reading from a specific pin. If there are specific parts of the Forth words not understood, then perhaps the Forth reference (http://www.newmicros.com/FORTH/HC11/V35/Alphabetical/Detailed/index.html) can be of help.

Think about the problem for a moment. How would you average values together? The example ADC averager (http://www.newmicros.com/isopod/appnotes/ADC.txt) essentially takes 1/20 of the change in readings to add into one displayed value.

: A/D ( -- )
RUN-AVG 2@ D>F FDUP RUN# @ S>F F/ F- F>D ANA0 S->D D+ RUN-AVG 2!
STARTCNV ; EEWORD

The A/D word starts by fetching (2@) the double number stored in RUN-AVG, then changing it into a floating point number, duplicating that floating point number (2 floats on float stack now), fetching the RUN# and changing it to a floating point (now on top of float stack) then dividing the top 2 numbers on the float stack, (which takes the previous RUN-AVG and divides by 20 placing the result on the float stack) then subtracting the top 2 numbers on the float stack (the result from the divide and previous duplicated float of RUN-AVG, placing the result of the subtraction back on the float stack) then changing that number from a float to a double (now on double stack) then using ANA0 to get a divided-by-8 analog reading which is converted into a double then added to the result and stored (2!) back in RUN-AVG. The STARTCNV just resets the ADC to get another reading.

[mbailey]

I know I'm being dumb but can you show me where to read from please, now I need to convert it into angles etc..

I have had the test code working but now need to make it working code..

SCRUB

HEX

2VARIABLE RUN-AVG EEWORD
VARIABLE RUN# EEWORD

DECIMAL
1.0E0 FCONSTANT SCALE EEWORD
HEX

: ADC_INIT ( -- )
2000 0E80 !
0003 0E81 !
3210 0E83 !
7654 0E84 !
0002 0E85 !

0.0 RUN-AVG 2!
20 RUN# !
; EEWORD

: STARTCNV ( -- )
2000 0E80 ! ; EEWORD

: 8/ 2/ 2/ 2/ ; EEWORD ( n -- n )

: ADC0 ( -- n )
E89 @ 8 /
; EEWORD

: A/D ( -- )
RUN-AVG 2@ D>F FDUP RUN# @ S>F F/ F- F>D ADC0 S->D D+ RUN-AVG 2!
STARTCNV ; EEWORD

: SMOOTHED. ( -- )
RUN-AVG 2@ D>F RUN# @ S>F F/ SCALE F* FDUP F.
FROUND F>D DROP .

; EEWORD


ADC_INIT

EVERY 50000 CYCLES SCHEDULE-RUNS A/D

Thanks in advance

Mike

[Dave]

First, there is a use of ADC0 in the program posted, that is already defined as the analog 0 pin address. It is not good practice to make words that are already in use by the Forth dictionary. In changing back to the original example using ANA0, and adding back in the TESTZ word,

DECIMAL
: TESTZ BEGIN CR ANA0 . SMOOTHED. ?TERMINAL UNTIL ; EEWORD

Typing TESTZ then quickly hitting a key to stop, gets a group of numbers like so :

2037 1986.0000 1986
2037 1986.0000 1986
2037 1986.0000 1986
1962 1987.5938 1988
1962 1987.5938 1988
1962 1987.5938 1988
1941 1986.7813 1987
1941 1986.7813 1987
1941 1986.7813 1987


The number in the first column is from ANA0 . , a raw dump of a 12 bit ADC0 number, what is typically seen bouncing around just taking readings.

Next column is from SMOOTHED. which is from the first F. showing a floating point average of the ADC readings, then the last column a rounded version of that floating point average.

All these numbers correspond to a 1.642 volt input (read by a meter) to the ADC0 pin. This would be a very similar voltage as that produced by the ADXRS300 when undisturbed. It was stated in the ADXRS300 data sheet that it produces a 5 mV per degree per second change when turned, indicating how quickly it is being turned. This 5 mV would be scaled to around 3.3 mV with the resistive divider. The program is reading a voltage level 100 times per second (5 million cycles divided by 50000) So if there is a change in the average analog voltage over a set period of time (1 reading, 5 readings, 200 readings) then a degree change might be calculated. A change of 3.3 mV, from a reading taken at point 0, to a reading taken 10 milliseconds later (100 times per second) would seem to indicate 100 degree per second turn rate. The 12 bit AD would read a 3.3 mV change as a 4 number difference, for example from 1985 to 1989. As the sensor does not respond that quickly (only 40 times per second response) it would seem that such a reading would be impossible or noise. So perhaps checking every 25 milliseconds (40 Hz) would give a more usable number. ( Using 62500 as the SCHEDULE-RUNS and checking every other pass). With checking every 25 milliseconds, a 3.3 mV change, indicated by a 4 number change in ADC reading would appear to correspond to a 40 degree change in orientation (3.3 mV over 1 second for a 1 degree change scaled to 1/40th of a second). It would seem from this reading that the sensor would be somewhat difficult to decipher an angle change from, in that any turns could also be attributed to readings bouncing slightly. I can't say how to change this into an angle measure as I don't have a gyro to empirically tell what conversion formula will best fit what is being read from the device. What might be tried is to take the rounded value generated in SMOOTHED. and multiply it by some constant to give you a change in degrees. This change could be added/subtracted to a prestored value, that maintains what the current orientation is. This is what I had attempted in an earlier example in this thread, but was unable to verify as I do not have the hardware in question. The actual numbers and formula to convert the final number given by SMOOTHED. to a degree measure will have to be empirically determined with the hardware being used.

It might be that an accelorometer is needed for orientation determination, with the gyro as a correcting factor, as used by others. This again would require the Kalman filter techniques to combine the input from the 2 sensors.

[mbailey]

Thanks,

I have played with the code a little and had it running 1-4 Average and it reacts far better now, I have plotted a graph and it only has a slight delay.

I need to make the SMOOTHED. just give me the smoothed reading so which part of RUN-AVG 2@ D>F RUN# @ S>F F/ SCALE F* FDUP F. do I use?

I have read through the words and tried to make sense of it, some is making sense but still struggling a little.

All the best

[Dave]

The whole word of SMOOTHED. from colon to semicolon is

: SMOOTHED. ( -- )
RUN-AVG 2@ D>F RUN# @ S>F F/ SCALE F* FDUP F.
FROUND F>D DROP .
; EEWORD

The F. takes a floating point number off the stack and outputs it to the screen out as
1962.1358
or something similar to that. The rounded number comes from the :
FROUND F>D DROP .
FROUND takes a floating point number off the float stack, rounds it, and places the rounded number back on the stack. F>D takes the floating point number off the float stack and converts it to a double length number, and places it on the regular stack. Because it is a double, it takes up 2 positions on the stack, so DROP is needed to get the zeros off first, then the remaining . (dot) outputs the remain single from the stack.

So in answer to your question, for SMOOTHED. to leave a single sized number on the stack, that can then be used to be manipulated by another word, you might try a modified version called, say SMOOTH-ONE

: SMOOTH-ONE ( -- 16b )
RUN-AVG 2@ D>F RUN# @ S>F F/ SCALE F* FROUND F>D DROP
; EEWORD

which would leave a 16 bit single on the stack. The FDUP and F. were removed, as well as the ending . to display numbers. You can display the results of SMOOTH-ONE by adding a . afterward, like
SMOOTH-ONE .

The 16 bit word on the stack can then be used by a later made math word, like
: DEGREE-CONV ( 16b --- )
( do some math here that gets close to observed change and display
; EEWORD

DEGREE-CONV would probably need to be looped at a regular interval, to have an accurate output of what is happening with the sensor in trying to measure angle change.

[mbailey]

Hi once again, I have that working lovely, With the code below I have 1-3 average and have added a Degree-conv with a simple peice of math the give me a zero point which works very well, when motionless the readings are fairly stable and hover over the zero mark by 1 to 2 values.

The problem is when I try and run DEGREE-CONV at the same rate as the Avg it stops the readings,
what am I doing wrong?

HEX

2VARIABLE RUN-AVG EEWORD
VARIABLE RUN# EEWORD
VARIABLE ANGLE EEWORD

DECIMAL
1.0E0 FCONSTANT SCALE EEWORD
HEX

: ADC_INIT ( -- )
2000 0E80 !
0003 0E81 !
3210 0E83 !
7654 0E84 !
0002 0E85 !

0.0 RUN-AVG 2!
3 RUN# !
; EEWORD

: STARTCNV ( -- )
2000 0E80 ! ; EEWORD

: 8/ 2/ 2/ 2/ ; EEWORD ( n -- n )

: ADC0 ( -- n )
E89 @ 2 /
; EEWORD

: INIT
90 ANGLE !
; EEWORD

: A/D ( -- )
RUN-AVG 2@ D>F FDUP RUN# @ S>F F/ F- F>D ADC0 S->D D+ RUN-AVG 2!
STARTCNV ; EEWORD

: SMOOTH-ONE ( -- 16b )
RUN-AVG 2@ D>F RUN# @ S>F F/ SCALE F* FROUND F>D DROP ; EEWORD

DECIMAL

: DEGREE-CONV SMOOTH-ONE 8108 - ; EEWORD

: TESTZ DECIMAL BEGIN CR ADC0 . SMOOTH-ONE . DEGREE-CONV . ?TERMINAL UNTIL ; EEWORD

ADC_INIT

INIT

EVERY 50000 CYCLES SCHEDULE-RUNS A/D
EVERY 50000 CYCLES SCHEDULE-RUNS DEGREE-CONV




If I remove EVERY 50000 CYCLES SCHEDULE-RUNS DEGREE-CONV

The readings work again with the TESTZ word.

Once this has over come I will need to in the DEGREE-CONV line perform some math as you say,

Am I right is saying:
I will need to take my given angle say 90Deg In the INIT and have the SMOOTH-ONE value added to it, but then it needs to be added to again by the SMOOTH-ONE value on top of the previous value so it accumulates??

This is ofcoarse once I have set the SMOOTH-ONE values to be equivalent to 3.36Mv per degree per second, which I suppose I could set up a GAIN control for....???

Sorry to be a pain!!!!

Thanks again

[Dave]

Only one SCHEDULE-RUNS statement will run at one time. SCHEDULE-RUNS will set up a loop that repeats the WORD designated every xx CYCLES. So if you want both A/D and DEGREE-CONV to be run every 50000 cycles, they need to together in a third word called maybe DO-BOTH

: DO-BOTH A/D DEGREE-CONV ; EEWORD

EVERY 50000 CYCLES SCHEDULE-RUNS DO-BOTH

Also, I would suggest using the ANA0 label for a word rather than the ADC0 word which is already defined in the system to reference a pin. Fixing this problem now can save other problems later.

An estimate of an undisturbed steady state value for the sensor needs to be measured, so that the averaged voltage that SMOOTH-ONE gives of the sensor voltage can be subtracted from it, to show what turn rate has been measured. Once the steady value is subtracted from the SMOOTH-ONE change value, the result (either positive of negative) can be summed into the value chosen as a starting angle. I think this was attempted with the 8108 - in DEG-CONV, but it may not be enough. There may be scaling of the voltage readings that needs to be done to reflect how often the readings are being taken (50000 CYCLES) as the voltage given reflects millivolts per degree per second. So a fetch of the current value stored in ANGLE needs to happen, then the degree value of the voltage reading needs to be added/subtracted to the value retrieved from ANGLE, and then the result needs to be stored back into ANGLE. Perhaps ANGLE-SUM can be made as follows

: ANGLE-SUM DEGREE-CONV ANGLE @ + ANGLE ! ;

which runs DEGREE-CONV to get a positive or negative value, then fetches the value currently stored in ANGLE, adds both on the stack, and then stores the stack back in ANGLE.

If you want to see the the current value of ANGLE, fetch it to be displayed by

ANGLE @ .

PS :

The number I would be expecting from DEGREE-CONV to add to ANGLE, should be showing up as mostly zeros if undisturbed, and small single digits as device is turned.

[mbailey]

Right I have been playing with that code and have it working great, I now have 0 when it starts and 11340 @ 360 Degrees Clockwise and vice versa. Also I have it so that when it returns to it's start point it is around the zero mark approx +- 8 which is about half a degree but if it is repeated in either direction is doesn't go mad, :D i.e. it doesn't keep on increasing or decreasing, it is just a little error which I am sure will be fine for the moment.
And as it is, I do not need to convert it into angles as I can use these figures.

Also I have noticed major Interference from my laptop near the sensors....

So now comes the fun part, I will need to integrate the gyro readings into the PID. But first I will play with the PID to understand it a little.

I have read through the PID and really need to have some brains here!, I am currently thinking about using Pots just to make it easier for the moment.

I will be using a single turn pot for both axis. it will be a 5K on the 3.3Vref,

How can I integrate Pots and sign magnitude to work in the same way?
I know its a deep question but I would appreciate the help once again..

Mike :cool:

[mbailey]

I should have also asked about the ADC channels,

In the averaging code it states ADC0 I wish to use 6 and 7 as I will be using 2 Gyros, it has a coding init to sample disable register what are these figures? 0002 0E85? what are the ones for 6 and 7?

If I write another code for the other gyro should it be the smae or should i write it all in one?

[Dave]

About the ADC channels : In the code, these have been set up for reading by setting the registers associated with the ADC. Information about the registers can be read in the User Manual for the DSP56F800 (http://www.freescale.com/files/dsp/doc/user_guide/DSP56F801-7UM.pdf) (big file). Table 3-11 on page 112 details the base address of the various peripherals available on the MCU. For the ADCA bank (which is for ADC0 to ADC7 available on the Plugapod) the address begins at 0E80 and apparently ends at 0EBF (note hexidecimal number base) Note in the ADC_INIT word that various registers in this range have numbers stored into them, to configure use of the ADC pins. This configuration affects all pins, so these can remain as is. The only things that needs to be changed is the ANA0 word, to address other pins. What is needed is to read the Results register for the appropriate pins used. On page 122 in Table 3-24 it gives HEX 9 A B C D E F and 10 to add to the base address of HEX E80 to read the results registers of pin ADC0 ADC1 ADC2 ADC3 ADC4 ADC5 ADC6 and ADC7. So 2 new words can be made called

: ANA6 E8F @ 8/ ; EEWORD

: ANA7 E90 @ 8/ ; EEWORD

That can be then used in more new words for each analog value that needs to be averaged.


About PID, pots, and Sign Magnitude, remember that the PWM-OUT word accepts a number between 0 and 65535 to represent the duty cycle output on a particular pin. A potentiometer would produce a voltage that is represented by a number when read by an ADC pin. Normally you might get 0 to 32760 from an ADC reading, but the ANA words above change that to the original 12 bit 0-4095 number that actually is read. Basically an ANA number can be used in a formula to get an output to send to PWM-OUT. Direction can be determined if the number is positive or negative (the absolute value of the number sent to PWM-OUT, sign modifies direction).

PID attempts a formula to get a particular response based on feedback. A strict Proportional response might take the a commanded 2048 value and check with the ANA word to get a difference between what is commanded and what is read to produce an error number, which is then directly multiplied by a constant P to produce an output sent to the PWM pins to drive towards that position. What is found is that this proportional correction can either get close to a commanded position but not quite reach it, or if too big a constant multiplier given, can overshoot and oscillate around the commanded position. That is where the D term comes in, which takes into account how the system is approaching the commanded signal. It takes the last error and the present error and subtracts the 2 to generate a Differential error that is multiplied by it's own constant. The Integral error, also multiplied by a constant, keeps a running record of errors divided by the number of errors recorded to help nudge a system to a correct position. Tuning the PID to get the correct constants is the difficult part, to be done by experimentation. Usually the P is increased until oscillation occurs, then D increased to reduce oscilllation, then I increased to get accuracy.

Adding the information retrieved from the gyro and other sources can verify position info, or add the pitch and angle of the whole system relative to the ground, where the pot would only give position relative to the framework the device is mounted on.

[mbailey]

Excellent thank you dave, which encoders do you use?
I have a few but they are SSI and too much trouble to write the software for them..

[Dave]

There are a number of quadrature encoders that have been used, usually HEDS by Agilent, US Digital has a variety, and some others that are escaping me. Usually they have already been mounted on the motor in question.

[mbailey]

Right more for you to have a think about,

I intended initially to use encoders for two reasons, firstly the rate gyro wasn't going to be on the moving platform, so I was going to measure the angular movement with the rate gyro and move the platform to the corrisponding value referencing the encoder..
The second was to enable me to know at which angle the platform is at for later developments of a GPS system.

Now I have thought of something slightly different, I wish to put the gyro sensor on the platfor which is driven by the output, which obviously means the gyro will rotate when the system is moved with a joystick.

Now the big question is, as stated in an earlier post i have the gyro working very well and no matter what you do to it when it returns back to is home angle it is very close to the values it had when it started, within about 10 increments which is equivalent to a third of a degree, which isn't too much of an error. so given these values can a PID code enable the platform to rotate and bring the values/platform back to the original figure?
Say to start with the value was 100 and it moves too 1000.

The problem I see in this is when I need to move the platform off its original point, the code will attept to bring it back, so it will need to be zero'd or updated when an input is made on the joystick, and maybe still working slightly whilst the input is being made, i.e. if i am moving the platform to the right and the base of the system moves to the right I need it to stay at the same velocity, if that makes sense. (If the base station moves to the right with no gyro and an input to the right is commanded, it will be faster than the given command.)

Thanks for your help so far you have been great..

Mike :)

[Dave]

I'm not sure I understand the question. If there is a set point that is to be driven to, there can be an error measured between that point and where a mechanism is. This error would be used in a PID calculation to generate an output to drive a motor, that drives the mechanism to the required point. Once it is there, no more error, and no more drive.

If driving back to a home value directed by the gyro output can be done, why couldn't the home value be changed to drive to another point?

Note the 10 increments error noted might be attributed to gyro drift. This is where I believe accelerometers and Kalman filtering come in, to correct for that drift.

[mbailey]

Have you ever seen or used a futaba heading hold gyro?, they hold the tail rotor of a model helicopter in one angle throughout its manouvers, but the user can very easily update that angle from his joystick, so I supose that would be resetting his home point. But whilst in movement it still needs to be correcting accelerations and deccelerations to keep the speed of rotation consistant.

How is this acheivable?

[Dave]

If the gyro detects a turn, then the controller should use the number from the gyro to produce a new home position. The voltage generated by the gyro could be used to set a new home position relative to the movement detected, so that the arm still points in one direction as motion is detected. PID would smooth the motion of the arm to the new home.

External inputs such as a joystick can also be added in to create a new home position. Still smoothed by the PID formula.

[mbailey]

Ok Now I don't understand..

If the axis is horizontal say 90 degrees and the number out of the gyro is 20, with external movement I want the gyro to move the motor to keep the output about 20, then if the user gives an input to say straight down and the gyro is kicking out 2855 i then want it to hold this number and so on.

[Dave]

If the gyro is outputting a value other than a middle voltage indicating rest, then the system is turning. When it turns, voltage greater or lesser than 2.5 volts is output. When the system stops, the output from the gyro goes back to 2.5. If there is a program that is keeping track of changes like has been discussed, the its final number may change to indicate different directions stopped at, like 20 or 2855 mentioned. But the gyro output is still indicating 2.5 volts when stopped.

What can be used is the delta signal from the gyro, to drive a motor in the opposite direction for holding a position. If a movement is detected, generating a voltage other than 2.5 on the gyro, then this can be used to signal the motor to move in an opposite direction to hold a value.

[mbailey]

Rather than using the raw data from the ADC channel, could I use the numbers from the code I already have, this way it would hold the heading rather than getting close,(which most gyros do).

[Dave]

Looking back to previous WORDS defined, something like the output from SMOOTHED. or SMOOTH-ONE would be usable to make a PWM command.

[mbailey]

sorry this is what I was talking about with the figures stated above.

How could I construct a code to do such a thing?

How would the home point be updated evrytime the user gives an input?

[Dave]

Above in a longer post there was mention about the "gyro returning to a home value". What is this home value? Is it stored in a variable somewhere? How is this number determined? Can a WORD set it to another "home number"? Perhaps by storing a different value in the variable that contains the home number? A number that can be re-set by an analog number read from the joystick attached to an analog input? And can a PID formula use this home number to drive a motor to a position like this home number, by using the encoders to tell how far away from it the motor is?

It might be helpful to list specifics of the WORDs currently in use, and the hardware currently attached and how it is attached.

[mbailey]

Ok,

The hardware is simply a rate gyro and a motor geared to a platform,

The gyro will be on the platform, so it will move with it when the motor runs,

The gyro readings are smoothed via the ADC filter previously talked about and then with the ANGLE @ Coding, it's giving me 11340 360 Degrees clockwise and -11340 360 Degrees anti clockwise, assuming that we start from 0.

So basically when joystick input is given, the platform with rotate and the gyro code will be calculating the new positions, say from gyro reading 0 (Horizontal) to gyro reading 2835 (vertical).

Now I want to be able to hold these individual headings, everytime the user inputs a command the heading will change and then I want the system to hold the new heading, so as above the code will be holding the ANGLE @ figure no matter what movement the whole system is under externally, I.e. you have the base system in your hands and are moving it around.

Then if you give an input via the joystick the platform will move untill you stop the command, now the ANGLE @ will be at say 2000 and now this figure needs to be held.

No encoders are being used currently, as I though I would be able to use the gyro as the encoder..

[Dave]

It appears that the ANGLE variable is the value that tells what position the device is in. When the value stored in ANGLE is fetched, with ANGLE @, then displayed with a dot . it tells what direction the platform is pointing.

What if the platform is moved to a new position, like the 2000 mentioned, then a zero 0 is stored in ANGLE at that point? Using something like
0 ANGLE !
Would not that reset the position to the new place that the platform had been pointed at? Could the turning measured by the gyro detecting movement of the whole then be measured off the new position, to provide a corrective signal to a motor? Instead of simply storing a zero in ANGLE, perhaps the ADC measured input from the joystick pot could be stored or added/subtracted into ANGLE, after being filtered, to have the platform commanded to a new position.

PID is based upon measuring an error signal. If the device is supposed to be pointing at zero (however zero has been made), then any positive of negative error is multiplied by a Proportional gain, an Integral gain, and a Differential gain. So the above ANGLE is supposed to go to zero, but ANGLE has been calculated to point elsewhere by inputs from the gyro telling where it is, and the joystick directing it to another "zero point". The Proportional simply uses the current value of ANGLE for it's output, the Derivative uses the difference between the last ANGLE and this ANGLE, and the Integral makes a running sum of recent errors (technically the sum of all errors, but this leads to a disproportionate weight of response) So the variables needed could be ANGLE, LAST-ANGLE, DIFF-ANGLE, and SUM-ANGLE. Constants needed could be K-PRO, K-DIFF, K-INT. ANGLE is easy, it is simply the current error. LAST-ANGLE would take on a value of zero initially, but after the PID had been calculated, take on the value of ANGLE. DIFF-ANGLE would be LAST-ANGLE minus ANGLE. SUM-ANGLE would add in ANGLE in each pass (sometimes negative, sometimes positive) but should be limited to a value to prevent Integral windup. A possible implementation of PID :

: CALC-MOTOR
ANGLE @ LAST-ANGLE @ - DIFF-ANGLE ! ( get current angle, last angle,
( subtract and store in DIFF-ANGLE
ANGLE @ SUM-ANGLE @ + SUM-ANGLE ! ( add in current ANGLE to SUM
SUM-ANGLE @ 10000 > IF 10000 SUM-ANGLE ! THEN ( limit upper end
SUM-ANGLE @ -10000 < IF -10000 SUM-ANGLE ! THEN ( limit lower end

K-PRO ANGLE @ * K-DIFF DIFF-ANGLE @ * K-INT SUM-ANGLE @ * + + P-OUT !
( calculates the output to send to PWM, to drive motor
ANGLE @ LAST-ANGLE ! ( update to a new last angle before next calculation
; EEWORD


The constants K-PRO, K-INT, and K-DIFF would all need to be adjusted to fit the responsiveness of the platform and motor. Constants are made and given a value initially at the same time :

12 CONSTANT K-PRO EEWORD

as shown in several example programs on the Download page. They do not require a fetch or store to use in a formula, like variables do. Above I'd added another variable called P-OUT to be used in driving the motor, such as
P-OUT @ PWMA0 PWM-OUT
which fetchs the PID calculated number to send to PWMA0. It seems both locked antiphase and sign mag drive had been discussed, so the number may need to be further adjusted to suit the drive method chosen.

As stated before, the value obtained from the gyro itself, which shows a movement even smoothed, could also be used in calculations to give a drive value to the PWM pin, to drive the motor as needed to compensate.