Merge branch 'master' of github.com:haldean/droidcopter
Conflicts:
tuning_DX.txt
Benjamin Bardin
9 years ago
230 | 230 | Thread.currentThread().setName("ChopperStatusImpl"); |
231 | 231 | File root = Environment.getExternalStorageDirectory(); |
232 | 232 | if (root == null) Log.e(TAG, "No root directory found"); |
233 | try { | |
233 | /*try { | |
234 | 234 | logwriter = new BufferedWriter(new FileWriter(root + "/chopper/" + logfilename, false)); |
235 | 235 | } catch (IOException e) { |
236 | 236 | Log.e(TAG, "No ChopperStatusImpl logfile"); |
237 | 237 | e.printStackTrace(); |
238 | } | |
238 | }*/ | |
239 | 239 | /* Register to receive battery status updates */ |
240 | 240 | BroadcastReceiver batteryInfo = new BroadcastReceiver() { |
241 | 241 | public void onReceive(Context context, Intent intent) { |
430 | 430 | setReadingField(ROLL, orientation[2] * -180.0 / Math.PI); |
431 | 431 | break; |
432 | 432 | case Sensor.TYPE_ROTATION_VECTOR: |
433 | Log.v(TAG, "my grav time: " + ((time - grav_time)/1000000)); | |
433 | //Log.v(TAG, "my grav time: " + ((time - grav_time)/1000000)); | |
434 | 434 | grav_time = time; |
435 | 435 | String timestring = Long.toString(time/1000000); |
436 | 436 | try { |
46 | 46 | public final static int CONNECTION_INTERVAL = 5000; |
47 | 47 | |
48 | 48 | /** URL of the control server */ |
49 | private final String mControl = new String("droidcopter.cs.columbia.edu"); | |
49 | //private final String mControl = new String("droidcopter.cs.columbia.edu"); | |
50 | private final String mControl = new String("192.168.168.65"); | |
50 | 51 | |
51 | 52 | /** Port used for text connection */ |
52 | 53 | private final int mTextOutPort = 7000; |
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 | } |
83 | 83 | check |
84 | 84 | elif [ 'nobuild' == "$1" ]; then |
85 | 85 | if [ '' == "$2" ]; then |
86 | export HOST=droidcopter.cs.columbia.edu | |
86 | # export HOST=droidcopter.cs.columbia.edu | |
87 | export HOST=192.168.168.65 | |
87 | 88 | else |
88 | 89 | export HOST=$2 |
89 | 90 | fi |
96 | 97 | clean |
97 | 98 | |
98 | 99 | if [ '' == "$1" ]; then |
99 | export HOST='droidcopter.cs.columbia.edu' | |
100 | # export HOST='droidcopter.cs.columbia.edu' | |
101 | export HOST='192.168.168.65' | |
100 | 102 | else |
101 | 103 | export HOST=$1 |
102 | 104 | fi |
0 | hello | |
0 | # Sat, 26 Jan 2013 21:59:02 | |
1 | # tuning DX select_num 3 expand_num 3 | |
2 | 2.3784428544460833E-6 9.796480402307306E-6 3.0761966382649954E-5 1.507204804879694E7 | |
3 | 6.8698377109233215E-6 8.577318298725533E-5 5.9414966198934354E-5 4.0994595984520875E7 | |
4 | 4.472655717262916E-5 4.608122303978246E-5 8.495135219704644E-5 2.147483647E9 | |
5 | 7.19599831204481E-5 5.442101138795286E-6 8.719780155987572E-5 2.147483647E9 | |
6 | # Sat, 26 Jan 2013 22:02:32 | |
7 | # tuning DX select_num 3 expand_num 3 | |
8 | 3.420062388233347E-6 2.8208014109574276E-5 5.335093945049906E-5 2.147483647E9 | |
9 | # Sat, 26 Jan 2013 22:04:11 | |
10 | # tuning DX select_num 3 expand_num 3 | |
11 | 5.2890877640935956E-5 6.751734943151931E-5 1.1510576724339528E-6 3.3050937539616935E7 | |
12 | 7.954887818075276E-5 6.04460235062575E-6 1.8345319724723674E-5 1.0863880453564493E7 | |
13 | 7.21681619387306E-5 8.71629291073021E-5 9.993396738956183E-5 3.1152117899106074E7 | |
14 | 6.024070815987459E-6 5.636652074046098E-5 5.014846605916668E-6 2.147483647E9 | |
15 | 2.5621724496714548E-5 4.978085160121574E-5 1.4324922470454338E-5 2.147483647E9 | |
16 | 8.123445312736881E-5 2.7545751763073302E-5 7.431612094178247E-5 2.147483647E9 | |
17 | 7.066148667794432E-5 1.1382954148749258E-5 2.9588293763502194E-5 1.087494456067241E7 | |
18 | 7.804313590391213E-5 1.6276525531706987E-5 9.441257671421533E-6 2.546852782139124E7 | |
19 | 6.26783973368529E-5 8.736468884579411E-5 9.962406453325058E-5 3.0818094590183195E7 | |
20 | # Sat, 26 Jan 2013 22:11:42 | |
21 | # tuning DX select_num 3 expand_num 3 | |
22 | 1.1661066860629655E-5 8.942418985595094E-5 9.456145867363559E-5 2.147483647E9 | |
23 | # Sat, 26 Jan 2013 22:13:04 | |
24 | # tuning DX select_num 3 expand_num 3 | |
25 | 7.874794160427424E-5 9.893574180441438E-5 3.524829760404918E-5 2.147483647E9 | |
26 | 8.262644307833829E-5 9.436511166339325E-5 7.326487764898568E-5 2.147483647E9 | |
27 | 1.699968869488825E-5 3.086632505783972E-5 1.397694397335313E-5 3.0257541173408236E7 | |
28 | 6.410982626434937E-5 3.8370672422643486E-5 4.1021856434621944E-5 2.8646952870915722E7 | |
29 | 6.352186590718209E-5 4.356333932797067E-5 1.0381862952417843E-5 3.1074259544017345E7 | |
30 | 9.473153375093021E-5 2.3809456147824626E-5 1.983457513587741E-5 2.9073889855807655E7 | |
31 | 6.594970085918967E-5 5.2396085326078835E-5 4.552905682493712E-5 2.8279947195559096E7 | |
32 | 2.8938102300648983E-5 5.483610746692185E-5 8.772391709172775E-5 3.0737138404519085E7 | |
33 | 5.6651586614229526E-5 5.428102071231426E-5 8.709025757866583E-5 1.4512682539575763E7 | |
34 | 7.878566341905075E-4 0.0012255041672445382 -0.0012336857863543293 3.165064576400225E7 | |
35 | 4.507864481953724E-4 -0.001322076943004094 2.734958224055095E-4 2.147483647E9 | |
36 | 2.3194837744717654E-4 0.0017733480428509566 -4.518599722737257E-4 8950283.26803798 | |
37 | 7.582142145080517E-5 7.264572185216025E-4 0.0010882164203169355 2.0305199515516195E7 | |
38 | -0.002828710319155888 6.308542689791321E-4 -0.001989105941607186 2.147483647E9 | |
39 | -1.4294034992194714E-4 0.0037895830194775233 -9.515964514826389E-4 1.9685751995149583E7 | |
40 | -0.002960030886272848 -0.0012305240706428824 -0.001464807832943751 2.147483647E9 | |
41 | 0.002959621470567161 0.004364361286731247 -4.2995067090811624E-4 2.147483647E9 | |
42 | -0.00123181099353837 -0.001015244701156473 -8.405918827622459E-4 2.147483647E9 | |
43 | 0.0013939172519243067 -0.001385010914338234 0.0018574363645458944 2.147483647E9 |