git.haldean.org droidcopter / 205afe8
Improving logging...and a test...:( Benjamin Bardin 8 years ago
3 changed file(s) with 79 addition(s) and 11 deletion(s). Raw diff Collapse all Expand all
100100 }
101101
102102 private int getNextCycle(int fromPos) {
103 int negativeIndex = fromPos;
103 int truncateStart = fromPos;
104 if (fromPos == 0) {
105 // Cycle a bit longer, to compensate for the weird start.
106 for (; mErrors.get(truncateStart) <= 0; truncateStart++) {
107 // If value at index is nonpositive, continue until positive.
108 if (truncateStart == mErrors.size()) return -1;
109 }
110 }
111 int negativeIndex = truncateStart;
104112 for (; mErrors.get(negativeIndex) >= 0; negativeIndex++) {
105113 // If value at index nonnegative, continue until negative.
106114 if (negativeIndex == mErrors.size()) return -1;
140148 // if (getScore() > other.getScore())
141149 return 1;
142150 }
151
152 public String gnuplotLine() {
153 return getP() + " " + getI() + " " + getD() + " " + getScore();
154 }
155
156 public static void main(String args[]) {
157 // Tests.
158 PidExperiment pe = new PidExperiment(0.0, 0.0, 0.0);
159 pe.addError(1);
160 pe.addError(-1);
161 pe.addError(1);
162 pe.addError(-1);
163 if (2 != pe.getNextCycle(0)) {
164 System.out.println("TEST #1 FAILED. Expected 2, got " + pe.getNextCycle(0));
165 System.exit(1);
166 }
167
168 pe = new PidExperiment(0.0, 0.0, 0.0);
169 pe.addError(-1);
170 pe.addError(1);
171 pe.addError(-1);
172 pe.addError(1);
173 pe.addError(-1);
174 if (3 != pe.getNextCycle(0)) {
175 System.out.println("TEST #2 FAILED. Expected 3, got " + pe.getNextCycle(0));
176 System.exit(1);
177 }
178
179 System.out.println("SUCCESS");
180 }
143181 }
4545
4646 public String[] processablePrefixes() {
4747 if (activated) {
48 return new String[] {"ACCEL", "MOTORSPEED", "GUID:PID:SET", "GUID:PID:VALUE"};
48 return new String[] {"GUID:ERROR", "MOTORSPEED", "GUID:PID:SET", "GUID:PID:VALUE"};
4949 } else {
5050 return new String[0];
5151 }
7676 }
7777 }
7878 }
79 }
79 }
11
22 import org.apache.commons.math3.distribution.MultivariateNormalDistribution;
33
4 import java.io.BufferedWriter;
5 import java.io.FileWriter;
6 import java.io.IOException;
7 import java.io.OutputStreamWriter;
8
9 import java.text.SimpleDateFormat;
410 import java.util.ArrayList;
11 import java.util.Date;
512 import java.util.PriorityQueue;
613 import java.util.Random;
7 import java.util.TreeSet;
14 import java.util.HashSet;
815
916 public class PidTuner implements Updatable {
1017 private static enum TuningAxis { DX, DY, DZ, DT };
2330 // Number of children to expand from each selected node.
2431 private static final int EXPAND_NUM = 3;
2532
33 private static SimpleDateFormat dateFormat =
34 new SimpleDateFormat("EEE, d MMM yyyy HH:mm:ss");
35
2636 private ArrayList<PidExperiment> mFringe;
27 private TreeSet<PidExperiment> mHistory;
37 private HashSet<PidExperiment> mHistory;
2838 private int mFringeIndex;
2939 // Covariance matrix for expanding nodes.
3040 private double[][] mExpCovar;
3141 private Random rn;
42 private BufferedWriter output;
3243
3344 private boolean mEnabled = false;
3445
4051 mAxis = TuningAxis.DX;
4152 } else if (axis.equals("dy")) {
4253 mAxis = TuningAxis.DY;
54 } else if (axis.equals("dt")) {
55 mAxis = TuningAxis.DT;
4356 } else {
4457 mEnabled = false;
4558 }
4861 return;
4962 }
5063 mFringe = new ArrayList<PidExperiment>();
51 mHistory = new TreeSet<PidExperiment>();
64 mHistory = new HashSet<PidExperiment>();
5265 mFringeIndex = 0;
5366 rn = new Random();
54
67 try {
68 output = new BufferedWriter(new FileWriter("tuning_" + mAxis + ".txt", true));
69 output.write("# "+ dateFormat.format(new Date()));
70 output.newLine();
71 output.write("# tuning " + mAxis + " select_num " + SELECT_NUM + " expand_num " +
72 EXPAND_NUM);
73 output.newLine();
74 } catch (IOException e) {
75 Debug.log("WARNING: PID TUNING LOGGING FAILED.");
76 e.printStackTrace();
77 }
5578 mExpCovar = new double[3][3];
5679 for (int i = 0; i < 3; i++) {
5780 for (int j = 0; j < 3; j++) {
84107 currentExp.addError(error);
85108 // If PidE not done, return;
86109 if (!currentExp.isDone()) return;
87 // PidE done: increment index
110 // PidE done: record result, increment index
111 try {
112 if (output != null) {
113 output.write(currentExp.gnuplotLine());
114 output.newLine();
115 output.flush();
116 }
117 } catch (IOException e) {
118 Debug.log("WARNING: PID TUNING LOGGING FAILED");
119 e.printStackTrace();
120 }
88121 mFringeIndex++;
89122 // If index <= mFringe.size(), send new PID values, return;
90123 if (mFringeIndex <= mFringe.size()) {
138171 mFringe.add(child);
139172 }
140173 }
141 // For debugging:
142 System.out.println("Best PID error so far: " + mHistory.first().getScore());
143 System.out.println(mHistory.first().getP() + ", " + mHistory.first().getI() + ", " + mHistory.first().getD());
144174 }
145175 }