After reading through some more research trying to understand the reason TriRot was not just perfectly stable when it lifted off for its first flight, I came across that old jewel that I last looked at when I was studying engineering, The PID controller, where the acronym stands for Proportional, Integral, Differential controller.
What I had originally was a 1:1 control system. If the IMU said I was tilting 5 degrees, I would correct exactly the same in the other direction. Looking back now, this does not account for the inertia of the system and any overshoot would be dealt with very slowly resulting in an unstable system (as evidenced by that last video ;-). Proportional ControlThis simply means that if the error is X you respond with X*Y where Y is the relative proportion you want to respond with. I.E., if Y = 1.5 and the error is 2 you will at that stage respond with 3. The idea being that you slightly exaggerate your response to get a quick correction. Y is referred to as the Gain Factor or Kp for Proportional Gain. Integral ControlHere we look at the time the error has been around. So you take the amplitude of the error and you multiply it with the time it’s been around. You also add a gain value to this for good measure (Ki). Effectively, you are working out the Area a graph is occupying. The graph here would be the output from the IMU. The best way to work out the area a graph occupies is to use integration. Differential ControlThis looks at the rate of change, or effectively the slope of the graph. This term also has a Gain factor, but in this case actually limiting the output, since your gain is generally in the 0.002 range. Take a look at the following articles which helped me make sense of it all: http://en.wikipedia.org/wiki/PID_controller http://en.wikipedia.org/wiki/Integral http://en.wikipedia.org/wiki/Differentiation_(mathematics) I don’t think I need to do full integration nor am I able to do full differentiation since I don’t know the function of the graph that will be generated by the random movements. So For integration I simply multiply the current amplitude with the timespan since the last loop and then sum that with previous calculations. For the differentiation, I again take the difference in amplitude (or y) and divide it by the difference in time (or again the timespan since the last loop). I’ve only just uploaded this to TriRot last night, spun up the motors (trying not to wake the kids) and moved it around a bit in my hand, but it seems allot more aggressive. I do still have to do the PID Tuning, which I’m expecting to be the hard part. Here is a Graph of a previous Pitch Test I took from TriRot. Effectively it’s just an output from the IMU in the Pitch axis, to which I have applied the PID algorithms in Excel. This is what convinced me that this might just work.  In this test:
|
Showing posts with label IMU. Show all posts
Showing posts with label IMU. Show all posts
Saturday, 14 June 2014
PID Control
Pitch Mixing (Video)
I recorded a quick video this morning showing me testing TriRot’s Pitch Mixing routines. I zip-tied a pivot beam to the chassis to limit movements to the Pitch axis only and also commented out the mixing code for Roll and Yaw.
In the video you can also see TriRot going through its full pre-flight self-check. Flashing the LED’s 5 times after the motors spin up for the first time means all is good and we are ready to go. I temporarily set channel 5 on my remote to be a kill switch. When this switch is toggled it either sends command to the motors and servo, or not. This is very handy for this part of the testing, where I’m not quite sure what TriRot will do as soon as it lifts off.
Below is a graph based on the data I captured during this test:
· The top blue line is both the left and right motor speeds.
· The Green line is the throttle
· The orange line shows the rear motor speed.
· The red line shows the input from the Received. It shows that I did not control TriRot from the Transmitter and that it is self-stabilising. (Notice that it is biased a bit forward in this graph and not on zero, because the pivot is a bit too far back so there is more weight in the front.)
· The blue line in the bottom shows the Yaw input from the IMU.
I have done exactly the same testing to sort out the Roll axis. Tonight (possibly) I will be testing the Yaw axis. I think this is going to be a bit more complicated because I need to be able to override the IMU Yaw over time; otherwise TriRot will always try to turn back into the direction it started up in. But, when I’m not applying any Yaw input it should hold in that direction.
Watch this space.
|
IMU Test - Video
So now that I have delved into the depths of 2’s compliment and also made sure I was using my incoming data in the correct order for the BitConverter, the Complementary filter from NuclearProjects.com, my IMU is working perfectly.
In the video I am using the Altitude indicator that Jesse wrote to test it with. (He even made the full source code available on CodeProject).
He has even implemented some calculations that now allows TriRot to track the z-Axis as well for Yaw control. Although this implementation is only based on Gyro’s (ITG-3200) and Accelerometers (ADXL345) ,which could drift over time, it is holding rock solid in tests so far.
The C# code that makes the above possible:
1: using System;
2: using Microsoft.SPOT;
3: using Tri_Rot_Version_1;
4: using System.Threading;
5: using GHIElectronics.NETMF.System;
6:
7: namespace IMUComplementaryFilter
8: {
9: public class Program
10: {
11:
12: private static Int32 gXZeroValue = 0;
13: private static Int32 gYZeroValue = 0;
14: private static Int32 gZZeroValue = 0;
15:
16: private static Int32 aXZeroValue = 0;
17: private static Int32 aYZeroValue = 0;
18: private static Int32 aZZeroValue = 0;
19: private const Double Rad2Deg = 57.2957795; // 1 radian = 57.2957795 degrees
20: private const Double Deg2Rad = 0.0174532925; // 0.0174532925 rads = 1 deg
21:
22:
23: public static void Main()
24: {
25: Debug.EnableGCMessages(false);
26: int i = 0;
27: Int16[] g;
28: Int16[] a;
29:
30: using (Gyro = new ITG3200I2C())
31: {
32: Gyro.StartUp();
33: }
34:
35: using (Acc = new ADXL345I2C())
36: {
37: Acc.StartUp();
38: }
39: Thread.Sleep(100);
40:
41: Int16 gXTemp = 0;
42: Int16 gYTemp = 0;
43: Int16 gZTemp = 0;
44:
45: Int16 aXTemp = 0;
46: Int16 aYTemp = 0;
47: Int16 aZTemp = 0;
48:
49: while (i < 100)
50: {
51: g = getGyroData();
52: a = getAccData();
53: Thread.Sleep(20);
54: i++;
55: }
56: i = 0;
57: while(i < 100)
58: {
59: g = getGyroData();
60: a = getAccData();
71:
72: gXTemp += g[0];
73: gYTemp += g[1];
74: gZTemp += g[2];
75:
76: aXTemp += a[0];
77: aYTemp += a[1];
78: aZTemp += a[2];
79:
80: Thread.Sleep(20);
81: i++;
82: }
83: gXZeroValue = (Int16)(gXTemp / 100F);
84: gYZeroValue = (Int16)(gYTemp / 100F);
85: gZZeroValue = (Int16)(gZTemp / 100F);
86:
87: aXZeroValue = (Int16)(aXTemp / 100F);
88: aYZeroValue = (Int16)(aYTemp / 100F);
89: aZZeroValue = (Int16)(aZTemp / 100F);
90:
91: Double GyroRateX = 0.0;
92: Double GyroRateY = 0.0;
93: Double GyroAngleX = 0.0;
94: Double GyroAngleY = 0.0;
95: Double GyroAngleZ = 0.0;
96:
97: Double AccelX = 0.0;
98: Double AccelY = 0.0;
99: Double AccelZ = 0.0;
100: Double AccelAngleX = 0.0;
101: Double AccelAngleY = 0.0;
102: Double AccelAngleZ = 0.0;
103: Double Roll = 0.0;
104: Double Pitch = 0.0;
105: Double Yaw = 0.0;
106:
107: Int16 minValx = -4095;
108: Int16 maxValx = 4095;
109: Int16 minValy = -4095;
110: Int16 maxValy = 4095;
111: Int16 minValz = -4095;
112: Int16 maxValz = 4095;
113:
114: long currentTime;
115: Single interval;
116: long lastTime = DateTime.Now.Ticks;
117:
118: long TicksPerMillisecond = TimeSpan.TicksPerMillisecond;
119: Single GyroAngleZ_dt = 0;
120:
121: Telemetry telemetry = new Telemetry();
122:
123:
124: while (true)
125: {
126: currentTime = DateTime.Now.Ticks;
127: interval = ((currentTime - lastTime) / TicksPerMillisecond)/1000F; //in milliseconds
128: lastTime = currentTime;
129:
130: g = getGyroData();
131: a = getAccData();
132:
133: GyroRateX = -1.0 * interval * (g[0] - gXZeroValue) / 14.375F;
134: GyroRateY = interval * (g[1] - gYZeroValue) / 14.375F;
135:
136: GyroAngleZ_dt = interval * (g[2] - gZZeroValue) / 14.375F;
137:
138: GyroAngleZ += -1.0 * GyroAngleZ_dt * (1 / (MathEx.Cos(Deg2Rad * Roll))); // convert Roll angle to Rads, find sin to use as scaler for Yaw
139:
140: if (GyroAngleZ < 0) GyroAngleZ += 360; // Keep within range of 0- 360 deg
141: if (GyroAngleZ >= 360) GyroAngleZ -= 360;
142: //----------------------------------------------------------------
143:
144: //convert read values to degrees -90 to 90 - Needed for atan2
145: Single xAng = map(a[0], minValx, maxValx, -90, 90);
146: Single yAng = map(a[1], minValy, maxValy, -90, 90);
147: Single zAng = map(a[2], minValz, maxValz, -90, 90);
148:
149: //Caculate 360deg values like so: atan2(-yAng, -zAng)
150: //atan2 outputs the value of -π to π (radians)
151: //We are then converting the radians to degrees
152: AccelAngleX = Rad2Deg * (MathEx.Atan2(-xAng, -zAng) + MathEx.PI);
153: AccelAngleY = Rad2Deg * (MathEx.Atan2(-yAng, -zAng) + MathEx.PI);
154:
155: // Keep angles between +-180deg
156: if (AccelAngleX > 180) AccelAngleX = AccelAngleX - 360;
157:
158: if (AccelAngleY <= 180) AccelAngleY = -1.0 * AccelAngleY;
159: if (AccelAngleY > 180) AccelAngleY = 360 - AccelAngleY;
160:
161: // Final values...
162: Roll = (0.98) * (Roll + GyroRateX) + (0.02) * (AccelAngleX);
163: Pitch = (0.98) * (Pitch + GyroRateY) + (0.02) * (AccelAngleY);
164: Yaw = GyroAngleZ;
165:
166: //Debug.Print(Roll.ToString() + "," + Pitch.ToString() + "," + Yaw.ToString());
167: telemetry.SendData(new TelemetryData(0, 0, 0, 0, (Single)Roll, (Single)Pitch, (Single)Yaw));
168: Thread.Sleep(50);
169: }
170: }
171:
172: private static Single map(Single x, Single in_min, Single in_max, Single out_min, Single out_max)
173: {
174: return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
175: }
176:
177: private static ITG3200I2C Gyro;
178: private static ADXL345I2C Acc;
179:
180: private static Int16[] getGyroData()
181: {
182: Int16[] data;
183: using (Gyro = new ITG3200I2C())
184: {
185: data = Gyro.AdcR();
186: }
187: return data;
188: }
189:
190: private static Int16[] getAccData()
191: {
192: Int16[] data;
193: using (Acc = new ADXL345I2C())
194: {
195: data = Acc.AdcR();
196: }
197: return data;
198: }
199:
200: }
201: }
I have now also finally updated my PartsList after noticing that I had never added the Gyro or the Accelerometer to the it.
Next steps would be to buy the Transmitter and Receiver pair and a PPM Encoder for interfacing into TriRot's Interface board.
|
Why 2’s compliment?
Why would chip manufacturers decide to provide output data in 2’s compliment? I’ve been having loads of issues with reading data from the ADXL345 and the ITF-3200.
At least the ITG-3200 has 16bit resolution so getting a 2’s complement value from it is as simple as using BitConverter.ToInt16().
The ADXL345 however provides only a maximum of 13bit resolution. Sending 13Bit data into BitConverter simply returns a positive number between 0 and 8191 instead of the correct 2’s compliment value of +-4095
Luckily I found StackExchange’s Electronics forum and some people in the know could set me straight. See 13-bit 2's Complement on .Net MF.
Why would they even do this? Surely it would be simpler to just output a UInt value between 0 and the Max Resolution. Surely it’s simpler to just apply an offset than to go through all the bit shifting. To make it worse it seems that different processors seem to deal with BitConverting differently. From example code that I’ve seen, the Arduino boards handle the Byte to Decimal conversion much easier than the .Net boards.
If the ADXL345 simply output data between 0 and 8191 I could just take that data and subtract 4095 and have my value.
In any event, I think I have made some progress again. I suspect I might also have my Most Significant Byte and my Least Significant Byte reversed when using BitConverter on either my Gyro's or the Accelerometers which is why I am getting very unstable data from my IMU for very small physical changes. Anyway, I hope to test this theory and also implement my new 2’s complement understanding tonight.
Beware of .Net Garbage Collector Debug Messages
Another issue I have resolved is with the .Net Garbage collector debug message. To date I have been running my code on my FEZmini board in debug mode. When you run the code via Visual Studio the output window shows the garbage collector output every couple of seconds.
I assumed that this processing would be suppressed if a debugger is not attached to the board (I.E. running it without Visual Studio). Alas, this is not the case. My IMU (while seemingly approaching some level of stability) would twitch every couple of seconds. This seems to be due to the Garbage collector debug code executing from time to time. 2 options here: Either disable the debug GC messages and/or run in Release mode.
Debug.EnableGCMessages(false);
The twitch is now gone, so hopefully once I have the 2’s compliment issue sorted out, everything should be working [better].
|
Busy couple of weeks
TriRot is taking a Diet
So last week I finally got TriRot fully assembled so obviously, the first thing was making the motors spin up for a test lift-off. Bear in mind there was no control, so I tied TriRot down giving it only enough tether to not destroy itself during a simple motor burn test. I slowly increased the motor speed until, sow-and-behold, TriRot was actually capable of lift-off. However at this stage the battery was not being carried, but I think it would still achieve lift-off regardless. My concern was how high I had to spin the motors before achieving lift-off, so I figured TriRot also needed to go on a diet.
TriRot: Before at 595grams including the battery.
TriRot after the first step of the diet dropping 36grams. Notice the motor and rotor mounts.
Only afterwards did I do a bit of research and realised that my battery was flat to below 9v, which is the minimum you should drop a LiPo too. But the diet could not have been bad either way. The next steps would be to cut out the sections of chassis which is not needed.
Once that was done I started working on getting some useful data out of my IMU. I spent quite a bit of time researching the Kalman filter as described by Starlino in his "Guide to using IMU in embedded applications", but he loses me in the Gyroscope section. I eventually ported the Arduino code which Fabio (using the same Gyro and Accelerometer) ported from Startlino's implementation (which I found afterwards) to C#.
Even then the output was more noisy that my raw accelerometer data. I eventually figured there was a problem in Fabio's code where he divided the raw Gyro data by 14.375?!? In Starlino's discussion, he mentions that getting degrees/second from your gyro you should be using the following formula:
RateAxz = (Axz1 – Axz0) / (t1 – t0).
Once I did this, my IMU seemed to be producing better data. The only way I could get this data though was to run in debug mode and then copy the data to a spread sheet. I figure the debug print routines screw up the timing etc., so I'm still not fully sure the code works as it should, but as soon as I can prove it does, I'll post the code to that in C#.
FEZmini 1.3 COM2
I've also been trying to make my TTL-232R-3V3-WE cable work on COM2, but I just couldn't. Anyway, I eventually tested the code with a loopback plug between Tx and Rx bot on FEZmini and on the Cable directly and both seem to actually work.
Anyway, an all-around busy couple of weeks.
|
Subscribe to:
Posts (Atom)