updates postrun
Benjamin Bardin
9 years ago
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 |
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 | } |
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()); |
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 |