git.haldean.org droidcopter / 0cea76e
Adds PID Tuner. Benjamin Bardin 9 years ago
4 changed file(s) with 293 addition(s) and 11 deletion(s). Raw diff Collapse all Expand all
0 package org.haldean.chopper.server;
1
2 import java.util.ArrayList;
3
4 public class PidExperiment implements Comparable<PidExperiment> {
5 public static final long MAX_TIME_MILLIS = 10000;
6 public static final double THRESHOLD_ANGLE = 20.0;
7 public static final double THRESHOLD_MULTIPLIER = 10000; // Exact value unimportant.
8
9 private double mP;
10 private double mI;
11 private double mD;
12 private ArrayList<Double> mErrors;
13 private long mStartTime;
14 private double mScore;
15
16 public PidExperiment(double p, double i, double d) {
17 mP = p;
18 mI = i;
19 mD = d;
20 mErrors = new ArrayList<Double>();
21 mScore = -1;
22 mStartTime = 0;
23 }
24
25 public double getP() {
26 return mP;
27 }
28
29 public double getI() {
30 return mI;
31 }
32
33 public double getD() {
34 return mD;
35 }
36
37 public void addError(double newError) {
38 if (mErrors.isEmpty()) {
39 mStartTime = System.currentTimeMillis();
40 }
41 mErrors.add(newError);
42 }
43
44 public boolean isDone() {
45 // True if time limit expired, or two full sin curves are complete.
46 if (isTimeUp()) return true;
47
48 int first = getNextCycle(0);
49 if (first == -1) return false;
50
51 int second = getNextCycle(first);
52 if (second == -1) return false;
53
54 int third = getNextCycle(second);
55 if (third == -1) return false;
56
57 return true;
58 }
59
60 public double getScore() {
61 // mScore saved once processed (lazy evaluation)
62 if (mScore != -1) return mScore;
63
64 if (isTimeUp()) {
65 mScore = Integer.MAX_VALUE;
66 return mScore;
67 }
68
69 int first = getNextCycle(0);
70 if (first == -1) {
71 mScore = Integer.MAX_VALUE;
72 return mScore;
73 }
74 int second = getNextCycle(first);
75 if (second == -1) {
76 mScore = Integer.MAX_VALUE;
77 return mScore;
78 }
79 int third = getNextCycle(second);
80 if (third == -1) {
81 mScore = Integer.MAX_VALUE;
82 return mScore;
83 }
84
85 double score = 0.0;
86 for (int i = first; i < third; i++) {
87 if (mErrors.get(i) < THRESHOLD_ANGLE) {
88 score += Math.abs(mErrors.get(i));
89 } else {
90 score += Math.abs(mErrors.get(i)) * THRESHOLD_MULTIPLIER;
91 }
92 }
93 mScore = score;
94 return mScore;
95 }
96
97 private int getNextCycle(int fromPos) {
98 int negativeIndex = fromPos;
99 for (; mErrors.get(negativeIndex) >= 0; negativeIndex++) {
100 // If value at index nonnegative, continue until negative.
101 if (negativeIndex == mErrors.size()) return -1;
102 }
103 int cycleStart = negativeIndex;
104 for (; mErrors.get(cycleStart) <= 0; cycleStart++) {
105 // If value at index is nonpositive, continue until positive.
106 if (negativeIndex == mErrors.size()) return -1;
107 }
108 return cycleStart;
109 }
110
111 private boolean isTimeUp() {
112 if (mStartTime == 0) return false;
113
114 if (System.currentTimeMillis() - mStartTime > MAX_TIME_MILLIS) {
115 return true;
116 } else {
117 return false;
118 }
119 }
120
121 /** Does not compare scores, only PID values. */
122 public boolean equals(PidExperiment other) {
123 return (getP() == other.getP()) && (getI() == other.getI()) && (getD() == other.getD());
124 }
125
126 public int compareTo(PidExperiment other) {
127 if (getScore() == other.getScore()) return 0;
128 if (getScore() < other.getScore()) return -1;
129 // if (getScore() > other.getScore())
130 return 1;
131 }
132 }
0 package org.haldean.chopper.server;
1
2 import org.apache.commons.math3.distribution.MultivariateNormalDistribution;
3
4 import java.util.ArrayList;
5 import java.util.PriorityQueue;
6 import java.util.Random;
7 import java.util.TreeSet;
8
9 public class PidTuner implements Updatable {
10 private static enum TuningAxis { DX, DY, DZ, DT };
11 // Axis to tune. Either DX or DY.
12 private TuningAxis mAxis;
13
14 // Search Parameters
15
16 // StDev for expanding nodes.
17 private static final double expStdev = 1.0e-6;
18 // Uniform distribution for creating initial nodes
19 private static final double initRangeStart = 0;
20 private static final double initRangeEnd = 1.0e-5;
21 // Number of nodes to select from fringe.
22 private static final int SELECT_NUM = 3;
23 // Number of children to expand from each selected node.
24 private static final int EXPAND_NUM = 3;
25
26 private ArrayList<PidExperiment> mFringe;
27 private TreeSet<PidExperiment> mHistory;
28 private int mFringeIndex;
29 // Covariance matrix for expanding nodes.
30 private double[][] mExpCovar;
31 private Random rn;
32
33 private boolean mEnabled = false;
34
35 public PidTuner() {
36 try {
37 String axis = ServerCreator.getArgument("pidTuning");
38 mEnabled = true;
39 if (axis.equals("dx")) {
40 mAxis = TuningAxis.DX;
41 } else if (axis.equals("dy")) {
42 mAxis = TuningAxis.DY;
43 } else {
44 mEnabled = false;
45 }
46 } catch (IllegalArgumentException e) {
47 // Default state; no tuning.
48 }
49 mFringe = new ArrayList<PidExperiment>();
50 mHistory = new TreeSet<PidExperiment>();
51 mFringeIndex = 0;
52 rn = new Random();
53
54 mExpCovar = new double[3][3];
55 for (int i = 0; i < 3; i++) {
56 for (int j = 0; j < 3; j++) {
57 if (i == j) {
58 mExpCovar[i][j] = expStdev;
59 } else {
60 mExpCovar[i][j] = 0.0;
61 }
62 }
63 }
64
65 // Create list of PidExperiments
66 double initRange = initRangeEnd - initRangeStart;
67 for (int j = 0; j < SELECT_NUM * EXPAND_NUM; j++) {
68 double p = rn.nextDouble() * initRange + initRangeStart;
69 double i = rn.nextDouble() * initRange + initRangeStart;
70 double d = rn.nextDouble() * initRange + initRangeStart;
71 mFringe.add(new PidExperiment(p, i, d));
72 }
73 }
74
75 public void update(String message) {
76 if (!mEnabled) return;
77 if (!message.startsWith("GUID:ERROR")) return;
78 // retrieve error for my axis
79 String parts[] = message.split(":");
80 Double error = new Double(parts[2 + mAxis.ordinal()]);
81 // Add error to current PidE
82 PidExperiment currentExp = mFringe.get(mFringeIndex);
83 currentExp.addError(error);
84 // If PidE not done, return;
85 if (!currentExp.isDone()) return;
86 // PidE done: increment index
87 mFringeIndex++;
88 // If index <= mFringe.size(), send new PID values, return;
89 if (mFringeIndex <= mFringe.size()) {
90 PidExperiment newExp = mFringe.get(mFringeIndex);
91 EnsignCrusher.tunePid(mAxis.ordinal(), 0, newExp.getP());
92 EnsignCrusher.tunePid(mAxis.ordinal(), 1, newExp.getI());
93 EnsignCrusher.tunePid(mAxis.ordinal(), 2, newExp.getD());
94 return;
95 }
96 // Fringe done: copy into history.
97 mHistory.addAll(mFringe);
98 mFringeIndex = 0;
99
100 // stochastically choose the nodes to expand.
101 ArrayList<PidExperiment> nodesToExpand = new ArrayList<PidExperiment>();
102 for (int i = 0; i < SELECT_NUM; i++) {
103 // the usual method of drawing a random element probabilistically.
104 double scoreSum = 0.0;
105 for (PidExperiment p : mFringe) {
106 scoreSum += p.getScore();
107 }
108 double pos = rn.nextDouble() * scoreSum;
109 double runningSum = 0.0;
110 for (int j = 0; j < mFringe.size(); j++) {
111 runningSum += mFringe.get(j).getScore();
112 if (runningSum >= pos) {
113 nodesToExpand.add(mFringe.get(j));
114 mFringe.remove(mFringe.get(j)); // To avoid duplicates.
115 break;
116 }
117 }
118 }
119 // Expand the selected nodes into new Fringe, checking against history.
120 mFringe.clear();
121 for (PidExperiment parent : nodesToExpand) {
122 double[] mean = new double[3];
123 mean[0] = parent.getP();
124 mean[1] = parent.getI();
125 mean[2] = parent.getD();
126 MultivariateNormalDistribution dist =
127 new MultivariateNormalDistribution(mean, mExpCovar);
128 for (int i = 0; i < EXPAND_NUM; i++) {
129 double[] sample = dist.sample();
130 PidExperiment child = new PidExperiment(sample[0], sample[1], sample[2]);
131 while (mHistory.contains(child)) {
132 // Extraordinarily unlikely under this implementation of expansion,
133 // but we'll check anyway in case the expansion implementation changes.
134 sample = dist.sample();
135 child = new PidExperiment(sample[0], sample[1], sample[2]);
136 }
137 mFringe.add(child);
138 }
139 }
140 // For debugging:
141 System.out.println("Best PID error so far: " + mHistory.first().getScore());
142 System.out.println(mHistory.first().getP() + ", " + mHistory.first().getI() + ", " + mHistory.first().getD());
143 }
144 }
4141
4242 public static String getArgument(String argumentName) throws IllegalArgumentException {
4343 if (! arguments.containsKey(argumentName)) {
44 throw new IllegalArgumentException(argumentName +
44 throw new IllegalArgumentException(argumentName +
4545 " was not specified on the command line.");
4646 }
4747
4848 return arguments.get(argumentName);
4949 }
5050
51 /** Run the chopper host
51 /** Run the chopper host
5252 * @param args -d enables printing debug information to the command line,
53 * and -h followed by a hostname specifies the hostname to connect to
53 * and -h followed by a hostname specifies the hostname to connect to
5454 * @throws Exception if the provided host name is invalid */
5555 public static void main(String args[]) {
5656 /* Parse command line arguments */
6363 arguments.put(argparts[0], value);
6464 }
6565 }
66
66
6767 if (arguments.containsKey("debug")) {
6868 Debug.setEnabled(true);
6969 if (arguments.containsKey("debuglog")) {
105105
106106 serverHost.accept();
107107 }
108 }
108 }
99 import javax.swing.event.*;
1010
1111 /** The superclass! This is the frame that encompasses everything
12 * else.
12 * else.
1313 * @author William Brown */
1414 public class ServerHost extends JFrame {
1515 /** The chopper name. We've been changing it enough that
2323 /** The component that displays the globe with tracking
2424 * data and location selection */
2525 final WorldWindComponent globeComponent;
26 /** The component that displays a 3D rendering of the
26 /** The component that displays a 3D rendering of the
2727 * current orientation of the chopper */
2828 final OrientationComponent orientationComponent;
2929 /** The component that displays telemetry and allows for
3434 /** The component that displays error values from the four PID
3535 * loops on the chopper. */
3636 final PidErrorComponent pidComponent;
37 final PidTuner pidTuner;
3738 /** An Updatable that receives all messages from the chopper
3839 * @see EchoUpdatable */
3940 final Updatable status;
7778 imagePanel = new ImagePanel();
7879 accelerationComponent = new AccelerationComponent();
7980 pidComponent = new PidErrorComponent();
81 pidTuner = new PidTuner();
8082 sensorComponent = new SensorComponent();
8183 status = new EchoUpdatable();
8284 motorComponent = new MotorComponent();
98100 dataReceiver.tie(PidTuningComponent.getInstance());
99101 dataReceiver.tieImage(imagePanel);
100102
103 dataReceiver.tie(pidTuner);
104
101105 MessageHookManager.addHook(motorComponent);
102106 MessageHookManager.addHook(sp);
103107
146150
147151 /** Initialize operating system specific stuff */
148152 public void osInit() {
149 Debug.log("Running on " + System.getProperty("os.name") + " " +
153 Debug.log("Running on " + System.getProperty("os.name") + " " +
150154 System.getProperty("os.arch"));
151155 if (System.getProperty("os.name").startsWith("Mac"))
152156 System.setProperty("apple.laf.useScreenMenuBar", "true");
156160 public void start() {
157161 /* Update the Look and Feel of components created
158162 * in the constructor */
159
163
160164 /* The right/left pane creator */
161165 JPanel horizontalPanel = new JPanel(new GridLayout(1,2));
162166
197201 disconnectButton.addActionListener(new ActionListener() {
198202 private boolean connected = true;
199203 public void actionPerformed(ActionEvent e) {
200 /* If connected, stop the DataReceiver and switch the
204 /* If connected, stop the DataReceiver and switch the
201205 * text of the button. If not connected, restart the
202206 * DataReceiver thread */
203207 if (connected) {
247251 new Thread(pad).start();
248252 }
249253 }
250 }
254 }