Commit 205afe89c9d5ef05e4d72b6a06d46288780303d0 - droidcopter

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

+39

-1

org/haldean/chopper/server/PidExperiment.java less more

100	100	}
101	101
102	102	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;
104	112	for (; mErrors.get(negativeIndex) >= 0; negativeIndex++) {
105	113	// If value at index nonnegative, continue until negative.
106	114	if (negativeIndex == mErrors.size()) return -1;

140	148	// if (getScore() > other.getScore())
141	149	return 1;
142	150	}
	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	}
143	181	}

-2

org/haldean/chopper/server/PidLogger.java less more

45	45
46	46	public String[] processablePrefixes() {
47	47	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"};
49	49	} else {
50	50	return new String[0];
51	51	}

76	76	}
77	77	}
78	78	}
79		}⏎
	79	}

+38

-8

org/haldean/chopper/server/PidTuner.java less more

1	1
2	2	import org.apache.commons.math3.distribution.MultivariateNormalDistribution;
3	3
	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;
4	10	import java.util.ArrayList;
	11	import java.util.Date;
5	12	import java.util.PriorityQueue;
6	13	import java.util.Random;
7		import java.util.TreeSet;
	14	import java.util.HashSet;
8	15
9	16	public class PidTuner implements Updatable {
10	17	private static enum TuningAxis { DX, DY, DZ, DT };

23	30	// Number of children to expand from each selected node.
24	31	private static final int EXPAND_NUM = 3;
25	32
	33	private static SimpleDateFormat dateFormat =
	34	new SimpleDateFormat("EEE, d MMM yyyy HH:mm:ss");
	35
26	36	private ArrayList<PidExperiment> mFringe;
27		private TreeSet<PidExperiment> mHistory;
	37	private HashSet<PidExperiment> mHistory;
28	38	private int mFringeIndex;
29	39	// Covariance matrix for expanding nodes.
30	40	private double[][] mExpCovar;
31	41	private Random rn;
	42	private BufferedWriter output;
32	43
33	44	private boolean mEnabled = false;
34	45

40	51	mAxis = TuningAxis.DX;
41	52	} else if (axis.equals("dy")) {
42	53	mAxis = TuningAxis.DY;
	54	} else if (axis.equals("dt")) {
	55	mAxis = TuningAxis.DT;
43	56	} else {
44	57	mEnabled = false;
45	58	}

48	61	return;
49	62	}
50	63	mFringe = new ArrayList<PidExperiment>();
51		mHistory = new TreeSet<PidExperiment>();
	64	mHistory = new HashSet<PidExperiment>();
52	65	mFringeIndex = 0;
53	66	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	}
55	78	mExpCovar = new double[3][3];
56	79	for (int i = 0; i < 3; i++) {
57	80	for (int j = 0; j < 3; j++) {

84	107	currentExp.addError(error);
85	108	// If PidE not done, return;
86	109	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	}
88	121	mFringeIndex++;
89	122	// If index <= mFringe.size(), send new PID values, return;
90	123	if (mFringeIndex <= mFringe.size()) {

138	171	mFringe.add(child);
139	172	}
140	173	}
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());
144	174	}
145	175	}