Commit d0f66c0459d5bda628a801deeafa64b762732335 - droidcopter

+6

-4

org/haldean/chopper/pilot/Guidance.java less more

45	45	/** The maximum change in motor speed permitted at one time if the chopper is stabilizing. Must be positive. */
46	46	public static final double MAX_DSTABLE = .1;
47	47
	48	public static final double MAX_AUTO_MOTOR = .67;
	49
48	50	/** Tag for logging */
49	51	public static final String TAG = new String("chopper.Guidance");
50	52

428	430	}
429	431	double t = mControlVars[3];
430	432
431		mMotorSpeed[0] = Math.sqrt(constrainValue(t - 2*y + z, 0, 1));
432		mMotorSpeed[1] = Math.sqrt(constrainValue(t + 2*y + z, 0, 1));
433		mMotorSpeed[2] = Math.sqrt(constrainValue(-t - 2*x + z, 0, 1));
434		mMotorSpeed[3] = Math.sqrt(constrainValue(-t + 2*x + z, 0, 1));
	433	mMotorSpeed[0] = Math.sqrt(constrainValue(t - 2*y + z, 0, MAX_AUTO_MOTOR));
	434	mMotorSpeed[1] = Math.sqrt(constrainValue(t + 2*y + z, 0, MAX_AUTO_MOTOR));
	435	mMotorSpeed[2] = Math.sqrt(constrainValue(-t - 2*x + z, 0, MAX_AUTO_MOTOR));
	436	mMotorSpeed[3] = Math.sqrt(constrainValue(-t + 2*x + z, 0, MAX_AUTO_MOTOR));
435	437	logArray("motorSpeeds", mMotorSpeed);
436	438	}
437	439

+6

-3

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

103	103	int truncateStart = fromPos;
104	104	if (fromPos == 0) {
105	105	// Cycle a bit longer, to compensate for the weird start.
	106	if (truncateStart >= mErrors.size()) return -1;
106	107	for (; mErrors.get(truncateStart) <= 0; truncateStart++) {
107	108	// If value at index is nonpositive, continue until positive.
108		if (truncateStart == mErrors.size()) return -1;
	109	if (truncateStart == mErrors.size() -1) return -1;
109	110	}
110	111	}
111	112	int negativeIndex = truncateStart;
	113	if (negativeIndex >= mErrors.size()) return -1;
112	114	for (; mErrors.get(negativeIndex) >= 0; negativeIndex++) {
113	115	// If value at index nonnegative, continue until negative.
114		if (negativeIndex == mErrors.size()) return -1;
	116	if (negativeIndex == mErrors.size() - 1) return -1;
115	117	}
116	118	int cycleStart = negativeIndex;
	119	if (cycleStart >= mErrors.size()) return -1;
117	120	for (; mErrors.get(cycleStart) <= 0; cycleStart++) {
118	121	// If value at index is nonpositive, continue until positive.
119		if (negativeIndex == mErrors.size()) return -1;
	122	if (cycleStart == mErrors.size() - 1) return -1;
120	123	}
121	124	return cycleStart;
122	125	}

+10

-1

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

29	29	private static final int SELECT_NUM = 3;
30	30	// Number of children to expand from each selected node.
31	31	private static final int EXPAND_NUM = 3;
	32	private static final int START_AFTER_N_CYCLES = 900;
	33	private int currentCycle = 0;
32	34
33	35	private static SimpleDateFormat dateFormat =
34	36	new SimpleDateFormat("EEE, d MMM yyyy HH:mm:ss");

94	96	double d = rn.nextDouble() * initRange + initRangeStart;
95	97	mFringe.add(new PidExperiment(p, i, d));
96	98	}
	99	System.out.println("setting pid to 0");
	100	EnsignCrusher.tunePid(mAxis.ordinal(), 0, 0.0);
	101	EnsignCrusher.tunePid(mAxis.ordinal(), 1, 0.0);
	102	EnsignCrusher.tunePid(mAxis.ordinal(), 2, 0.0);
97	103	}
98	104
99	105	public void update(String message) {
100	106	if (!mEnabled) return;
101	107	if (!message.startsWith("GUID:ERROR")) return;
	108	currentCycle++;
	109	if (currentCycle < START_AFTER_N_CYCLES) return;
102	110	// retrieve error for my axis
103	111	String parts[] = message.split(":");
104	112	Double error = new Double(parts[2 + mAxis.ordinal()]);

118	126	Debug.log("WARNING: PID TUNING LOGGING FAILED");
119	127	e.printStackTrace();
120	128	}
	129	System.out.println("New experiment, cycle " + currentCycle);
121	130	mFringeIndex++;
122	131	// If index <= mFringe.size(), send new PID values, return;
123		if (mFringeIndex <= mFringe.size()) {
	132	if (mFringeIndex < mFringe.size()) {
124	133	PidExperiment newExp = mFringe.get(mFringeIndex);
125	134	EnsignCrusher.tunePid(mAxis.ordinal(), 0, newExp.getP());
126	135	EnsignCrusher.tunePid(mAxis.ordinal(), 1, newExp.getI());

+3

-2

run less more

36	36	CMD="java -Djava.library.path=/lib/:jars/worldwind/:jars/ -Xmx512m
37	37	-Dsun.java2d.noddraw=true $DOCK org.haldean.chopper.server.ServerCreator
38	38	host=$HOST port=$PORT $DEBUG $HEARTLESS pidlog=pidlog.txt imgdir=video/
39		debuglog=debug.txt pidTuning=dx"
	39	debuglog=debug.txt"
40	40	echo $CMD
41	41	exec $CMD
42	42	}

97	97	clean
98	98
99	99	if [ '' == "$1" ]; then
100		export HOST='droidcopter.cs.columbia.edu'
	100	# export HOST='droidcopter.cs.columbia.edu'
	101	export HOST='192.168.168.65'
101	102	else
102	103	export HOST=$1
103	104	fi