Merge branch 'master' of github.com:haldean/droidcopter
Benjamin Bardin
9 years ago
Binary diff not shown
0 | package org.haldean.chopper.server; | |
1 | ||
2 | import java.util.ArrayList; | |
3 | ||
4 | /** | |
5 | * Note: this class has a natural ordering that is inconsistent with equals. | |
6 | */ | |
7 | public class PidExperiment implements Comparable<PidExperiment> { | |
8 | public static final long MAX_TIME_MILLIS = 10000; | |
9 | public static final double THRESHOLD_ANGLE = 20.0; | |
10 | public static final double THRESHOLD_MULTIPLIER = 10000; // Exact value unimportant. | |
11 | ||
12 | private double mP; | |
13 | private double mI; | |
14 | private double mD; | |
15 | private ArrayList<Double> mErrors; | |
16 | private long mStartTime; | |
17 | private double mScore; | |
18 | ||
19 | public PidExperiment(double p, double i, double d) { | |
20 | mP = p; | |
21 | mI = i; | |
22 | mD = d; | |
23 | mErrors = new ArrayList<Double>(); | |
24 | mScore = -1; | |
25 | mStartTime = 0; | |
26 | } | |
27 | ||
28 | public double getP() { | |
29 | return mP; | |
30 | } | |
31 | ||
32 | public double getI() { | |
33 | return mI; | |
34 | } | |
35 | ||
36 | public double getD() { | |
37 | return mD; | |
38 | } | |
39 | ||
40 | public void addError(double newError) { | |
41 | if (mErrors.isEmpty()) { | |
42 | mStartTime = System.currentTimeMillis(); | |
43 | } | |
44 | mErrors.add(newError); | |
45 | } | |
46 | ||
47 | public boolean isDone() { | |
48 | // True if time limit expired, or two full sin curves are complete. | |
49 | if (isTimeUp()) return true; | |
50 | ||
51 | int first = getNextCycle(0); | |
52 | if (first == -1) return false; | |
53 | ||
54 | int second = getNextCycle(first); | |
55 | if (second == -1) return false; | |
56 | ||
57 | int third = getNextCycle(second); | |
58 | if (third == -1) return false; | |
59 | ||
60 | return true; | |
61 | } | |
62 | ||
63 | public double getScore() { | |
64 | // mScore saved once processed (lazy evaluation) | |
65 | if (mScore != -1) return mScore; | |
66 | ||
67 | // If we don't get to the start of a third cycle, this experiment | |
68 | // was really bad. Return MAX_VALUE. | |
69 | if (isTimeUp()) { | |
70 | mScore = Integer.MAX_VALUE; | |
71 | return mScore; | |
72 | } | |
73 | ||
74 | int first = getNextCycle(0); | |
75 | if (first == -1) { | |
76 | mScore = Integer.MAX_VALUE; | |
77 | return mScore; | |
78 | } | |
79 | int second = getNextCycle(first); | |
80 | if (second == -1) { | |
81 | mScore = Integer.MAX_VALUE; | |
82 | return mScore; | |
83 | } | |
84 | int third = getNextCycle(second); | |
85 | if (third == -1) { | |
86 | mScore = Integer.MAX_VALUE; | |
87 | return mScore; | |
88 | } | |
89 | ||
90 | double score = 0.0; | |
91 | for (int i = first; i < third; i++) { | |
92 | if (mErrors.get(i) < THRESHOLD_ANGLE) { | |
93 | score += Math.abs(mErrors.get(i)); | |
94 | } else { | |
95 | score += Math.abs(mErrors.get(i)) * THRESHOLD_MULTIPLIER; | |
96 | } | |
97 | } | |
98 | mScore = score; | |
99 | return mScore; | |
100 | } | |
101 | ||
102 | private int getNextCycle(int 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; | |
112 | for (; mErrors.get(negativeIndex) >= 0; negativeIndex++) { | |
113 | // If value at index nonnegative, continue until negative. | |
114 | if (negativeIndex == mErrors.size()) return -1; | |
115 | } | |
116 | int cycleStart = negativeIndex; | |
117 | for (; mErrors.get(cycleStart) <= 0; cycleStart++) { | |
118 | // If value at index is nonpositive, continue until positive. | |
119 | if (negativeIndex == mErrors.size()) return -1; | |
120 | } | |
121 | return cycleStart; | |
122 | } | |
123 | ||
124 | private boolean isTimeUp() { | |
125 | if (mStartTime == 0) return false; | |
126 | ||
127 | if (System.currentTimeMillis() - mStartTime > MAX_TIME_MILLIS) { | |
128 | return true; | |
129 | } else { | |
130 | return false; | |
131 | } | |
132 | } | |
133 | ||
134 | /** Does not compare scores, only PID values. */ | |
135 | public boolean equals(PidExperiment other) { | |
136 | return (getP() == other.getP()) && (getI() == other.getI()) && (getD() == other.getD()); | |
137 | } | |
138 | ||
139 | public int hashCode() { | |
140 | return new Double(getP()).hashCode() ^ | |
141 | new Double(getI()).hashCode() ^ | |
142 | new Double(getD()).hashCode(); | |
143 | } | |
144 | ||
145 | public int compareTo(PidExperiment other) { | |
146 | if (getScore() == other.getScore()) return 0; | |
147 | if (getScore() < other.getScore()) return -1; | |
148 | // if (getScore() > other.getScore()) | |
149 | return 1; | |
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 | } | |
181 | } |
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 | } |
0 | package org.haldean.chopper.server; | |
1 | ||
2 | import org.apache.commons.math3.distribution.MultivariateNormalDistribution; | |
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; | |
10 | import java.util.ArrayList; | |
11 | import java.util.Date; | |
12 | import java.util.PriorityQueue; | |
13 | import java.util.Random; | |
14 | import java.util.HashSet; | |
15 | ||
16 | public class PidTuner implements Updatable { | |
17 | private static enum TuningAxis { DX, DY, DZ, DT }; | |
18 | // Axis to tune. Either DX or DY. | |
19 | private TuningAxis mAxis; | |
20 | ||
21 | // Search Parameters | |
22 | ||
23 | // StDev for expanding nodes. | |
24 | private static final double expStdev = 2.0e-6; | |
25 | // Uniform distribution for creating initial nodes | |
26 | private static final double initRangeStart = 0; | |
27 | private static final double initRangeEnd = 1.0e-4; | |
28 | // Number of nodes to select from fringe. | |
29 | private static final int SELECT_NUM = 3; | |
30 | // Number of children to expand from each selected node. | |
31 | private static final int EXPAND_NUM = 3; | |
32 | ||
33 | private static SimpleDateFormat dateFormat = | |
34 | new SimpleDateFormat("EEE, d MMM yyyy HH:mm:ss"); | |
35 | ||
36 | private ArrayList<PidExperiment> mFringe; | |
37 | private HashSet<PidExperiment> mHistory; | |
38 | private int mFringeIndex; | |
39 | // Covariance matrix for expanding nodes. | |
40 | private double[][] mExpCovar; | |
41 | private Random rn; | |
42 | private BufferedWriter output; | |
43 | ||
44 | private boolean mEnabled = false; | |
45 | ||
46 | public PidTuner() { | |
47 | try { | |
48 | String axis = ServerCreator.getArgument("pidTuning"); | |
49 | mEnabled = true; | |
50 | if (axis.equals("dx")) { | |
51 | mAxis = TuningAxis.DX; | |
52 | } else if (axis.equals("dy")) { | |
53 | mAxis = TuningAxis.DY; | |
54 | } else if (axis.equals("dt")) { | |
55 | mAxis = TuningAxis.DT; | |
56 | } else { | |
57 | mEnabled = false; | |
58 | } | |
59 | } catch (IllegalArgumentException e) { | |
60 | // Default state; no tuning. | |
61 | return; | |
62 | } | |
63 | mFringe = new ArrayList<PidExperiment>(); | |
64 | mHistory = new HashSet<PidExperiment>(); | |
65 | mFringeIndex = 0; | |
66 | rn = new Random(); | |
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 | } | |
78 | mExpCovar = new double[3][3]; | |
79 | for (int i = 0; i < 3; i++) { | |
80 | for (int j = 0; j < 3; j++) { | |
81 | if (i == j) { | |
82 | mExpCovar[i][j] = expStdev; | |
83 | } else { | |
84 | mExpCovar[i][j] = 0.0; | |
85 | } | |
86 | } | |
87 | } | |
88 | ||
89 | // Create list of PidExperiments | |
90 | double initRange = initRangeEnd - initRangeStart; | |
91 | for (int j = 0; j < SELECT_NUM * EXPAND_NUM; j++) { | |
92 | double p = rn.nextDouble() * initRange + initRangeStart; | |
93 | double i = rn.nextDouble() * initRange + initRangeStart; | |
94 | double d = rn.nextDouble() * initRange + initRangeStart; | |
95 | mFringe.add(new PidExperiment(p, i, d)); | |
96 | } | |
97 | } | |
98 | ||
99 | public void update(String message) { | |
100 | if (!mEnabled) return; | |
101 | if (!message.startsWith("GUID:ERROR")) return; | |
102 | // retrieve error for my axis | |
103 | String parts[] = message.split(":"); | |
104 | Double error = new Double(parts[2 + mAxis.ordinal()]); | |
105 | // Add error to current PidE | |
106 | PidExperiment currentExp = mFringe.get(mFringeIndex); | |
107 | currentExp.addError(error); | |
108 | // If PidE not done, return; | |
109 | if (!currentExp.isDone()) return; | |
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 | } | |
121 | mFringeIndex++; | |
122 | // If index <= mFringe.size(), send new PID values, return; | |
123 | if (mFringeIndex <= mFringe.size()) { | |
124 | PidExperiment newExp = mFringe.get(mFringeIndex); | |
125 | EnsignCrusher.tunePid(mAxis.ordinal(), 0, newExp.getP()); | |
126 | EnsignCrusher.tunePid(mAxis.ordinal(), 1, newExp.getI()); | |
127 | EnsignCrusher.tunePid(mAxis.ordinal(), 2, newExp.getD()); | |
128 | return; | |
129 | } | |
130 | // Fringe done: copy into history. | |
131 | mHistory.addAll(mFringe); | |
132 | mFringeIndex = 0; | |
133 | ||
134 | // stochastically choose the nodes to expand. | |
135 | ArrayList<PidExperiment> nodesToExpand = new ArrayList<PidExperiment>(); | |
136 | for (int i = 0; i < SELECT_NUM; i++) { | |
137 | // the usual method of drawing a random element probabilistically. | |
138 | double scoreSum = 0.0; | |
139 | for (PidExperiment p : mFringe) { | |
140 | scoreSum += p.getScore(); | |
141 | } | |
142 | double pos = rn.nextDouble() * scoreSum; | |
143 | double runningSum = 0.0; | |
144 | for (int j = 0; j < mFringe.size(); j++) { | |
145 | runningSum += mFringe.get(j).getScore(); | |
146 | if (runningSum >= pos) { | |
147 | nodesToExpand.add(mFringe.get(j)); | |
148 | mFringe.remove(mFringe.get(j)); // To avoid duplicates. | |
149 | break; | |
150 | } | |
151 | } | |
152 | } | |
153 | // Expand the selected nodes into new Fringe, checking against history. | |
154 | mFringe.clear(); | |
155 | for (PidExperiment parent : nodesToExpand) { | |
156 | double[] mean = new double[3]; | |
157 | mean[0] = parent.getP(); | |
158 | mean[1] = parent.getI(); | |
159 | mean[2] = parent.getD(); | |
160 | MultivariateNormalDistribution dist = | |
161 | new MultivariateNormalDistribution(mean, mExpCovar); | |
162 | for (int i = 0; i < EXPAND_NUM; i++) { | |
163 | double[] sample = dist.sample(); | |
164 | PidExperiment child = new PidExperiment(sample[0], sample[1], sample[2]); | |
165 | while (mHistory.contains(child)) { | |
166 | // Extraordinarily unlikely under this implementation of expansion, | |
167 | // but we'll check anyway in case the expansion implementation changes. | |
168 | sample = dist.sample(); | |
169 | child = new PidExperiment(sample[0], sample[1], sample[2]); | |
170 | } | |
171 | mFringe.add(child); | |
172 | } | |
173 | } | |
174 | } | |
175 | } |
41 | 41 | |
42 | 42 | public static String getArgument(String argumentName) throws IllegalArgumentException { |
43 | 43 | if (! arguments.containsKey(argumentName)) { |
44 | throw new IllegalArgumentException(argumentName + | |
44 | throw new IllegalArgumentException(argumentName + | |
45 | 45 | " was not specified on the command line."); |
46 | 46 | } |
47 | 47 | |
48 | 48 | return arguments.get(argumentName); |
49 | 49 | } |
50 | 50 | |
51 | /** Run the chopper host | |
51 | /** Run the chopper host | |
52 | 52 | * @param args -d enables printing debug information to the command line, |
53 | * and -h followed by a hostname specifies the hostname to connect to | |
53 | * and -h followed by a hostname specifies the hostname to connect to | |
54 | 54 | * @throws Exception if the provided host name is invalid */ |
55 | 55 | public static void main(String args[]) { |
56 | 56 | /* Parse command line arguments */ |
63 | 63 | arguments.put(argparts[0], value); |
64 | 64 | } |
65 | 65 | } |
66 | ||
66 | ||
67 | 67 | if (arguments.containsKey("debug")) { |
68 | 68 | Debug.setEnabled(true); |
69 | 69 | if (arguments.containsKey("debuglog")) { |
105 | 105 | |
106 | 106 | serverHost.accept(); |
107 | 107 | } |
108 | }⏎ | |
108 | } |
9 | 9 | import javax.swing.event.*; |
10 | 10 | |
11 | 11 | /** The superclass! This is the frame that encompasses everything |
12 | * else. | |
12 | * else. | |
13 | 13 | * @author William Brown */ |
14 | 14 | public class ServerHost extends JFrame { |
15 | 15 | /** The chopper name. We've been changing it enough that |
23 | 23 | /** The component that displays the globe with tracking |
24 | 24 | * data and location selection */ |
25 | 25 | final WorldWindComponent globeComponent; |
26 | /** The component that displays a 3D rendering of the | |
26 | /** The component that displays a 3D rendering of the | |
27 | 27 | * current orientation of the chopper */ |
28 | 28 | final OrientationComponent orientationComponent; |
29 | 29 | /** The component that displays telemetry and allows for |
34 | 34 | /** The component that displays error values from the four PID |
35 | 35 | * loops on the chopper. */ |
36 | 36 | final PidErrorComponent pidComponent; |
37 | final PidTuner pidTuner; | |
37 | 38 | /** An Updatable that receives all messages from the chopper |
38 | 39 | * @see EchoUpdatable */ |
39 | 40 | final Updatable status; |
77 | 78 | imagePanel = new ImagePanel(); |
78 | 79 | accelerationComponent = new AccelerationComponent(); |
79 | 80 | pidComponent = new PidErrorComponent(); |
81 | pidTuner = new PidTuner(); | |
80 | 82 | sensorComponent = new SensorComponent(); |
81 | 83 | status = new EchoUpdatable(); |
82 | 84 | motorComponent = new MotorComponent(); |
98 | 100 | dataReceiver.tie(PidTuningComponent.getInstance()); |
99 | 101 | dataReceiver.tieImage(imagePanel); |
100 | 102 | |
103 | dataReceiver.tie(pidTuner); | |
104 | ||
101 | 105 | MessageHookManager.addHook(motorComponent); |
102 | 106 | MessageHookManager.addHook(sp); |
103 | 107 | |
146 | 150 | |
147 | 151 | /** Initialize operating system specific stuff */ |
148 | 152 | public void osInit() { |
149 | Debug.log("Running on " + System.getProperty("os.name") + " " + | |
153 | Debug.log("Running on " + System.getProperty("os.name") + " " + | |
150 | 154 | System.getProperty("os.arch")); |
151 | 155 | if (System.getProperty("os.name").startsWith("Mac")) |
152 | 156 | System.setProperty("apple.laf.useScreenMenuBar", "true"); |
156 | 160 | public void start() { |
157 | 161 | /* Update the Look and Feel of components created |
158 | 162 | * in the constructor */ |
159 | ||
163 | ||
160 | 164 | /* The right/left pane creator */ |
161 | 165 | JPanel horizontalPanel = new JPanel(new GridLayout(1,2)); |
162 | 166 | |
197 | 201 | disconnectButton.addActionListener(new ActionListener() { |
198 | 202 | private boolean connected = true; |
199 | 203 | public void actionPerformed(ActionEvent e) { |
200 | /* If connected, stop the DataReceiver and switch the | |
204 | /* If connected, stop the DataReceiver and switch the | |
201 | 205 | * text of the button. If not connected, restart the |
202 | 206 | * DataReceiver thread */ |
203 | 207 | if (connected) { |
247 | 251 | new Thread(pad).start(); |
248 | 252 | } |
249 | 253 | } |
250 | }⏎ | |
254 | } |
11 | 11 | fi |
12 | 12 | |
13 | 13 | export ANDROIDPATH="$ANDROID_ROOT/platforms/android-8/android.jar" |
14 | export CLASSPATH=$ANDROIDPATH:$CLASSPATH:./lib/simplegraph/:`ls -1 jars/*.jar jars/worldwind/*.jar | xargs | sed "s/ /:/g"`:/usr/share/java/j3dcore.jar:/usr/share/java/j3dutils.jar:/usr/share/java/vecmath.jar:. | |
14 | export CLASSPATH=$ANDROIDPATH:$CLASSPATH:./lib/simplegraph/:`ls -1 jars/*.jar jars/worldwind/*.jar | xargs | sed "s/ /:/g"`:/usr/share/java/j3dcore.jar:/usr/share/java/j3dutils.jar:/usr/share/java/vecmath.jar:.:/usr/lib/jvm/java-7-openjdk/jre/lib/ext/j3dcore.jar:/usr/lib/jvm/java-7-openjdk/jre/lib/ext/j3dutils.jar:/usr/lib/jvm/java-7-openjdk/jre/lib/ext/vecmath.jar: | |
15 | 15 | export PORT=7000 |
16 | 16 | |
17 | 17 | if [ 'heartless' == "$1" ] || [ 'heartless' == "$2" ]; then |
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" | |
39 | debuglog=debug.txt pidTuning=dx" | |
40 | 40 | echo $CMD |
41 | 41 | exec $CMD |
42 | 42 | } |
52 | 52 | } |
53 | 53 | |
54 | 54 | build() { |
55 | javac org/haldean/chopper/server/ServerCreator.java -Xlint:deprecation | |
55 | javac org/haldean/chopper/server/ServerCreator.java -Xlint:deprecation -encoding ISO-8859-1 | |
56 | 56 | } |
57 | 57 | |
58 | 58 | keyboard() { |
Binary diff not shown
0 | \input{latex_template.tex} | |
0 | \documentclass[letterpaper]{article} | |
1 | \usepackage{amsmath,amssymb,appendix,color,etoolbox, | |
2 | graphicx,index,listings,lscape,natbib,url} | |
3 | ||
4 | \usepackage{mathpazo} % math & rm | |
5 | \linespread{1.05} % Palatino needs more leading (space between lines) | |
6 | \usepackage[scaled]{helvet} % ss | |
7 | \normalfont | |
8 | \usepackage[T1]{fontenc} | |
9 | ||
10 | \newcommand\figref[1]{Figure \ref{fig:#1}} | |
11 | \newcommand\tabref[1]{Table \ref{tab:#1}} | |
12 | \newcommand\code[1]{\texttt{#1}} | |
13 | \newcommand\pipe[0]{\;|\;} | |
14 | \newcommand\st[0]{\;\mathrm{s.t.}\;} | |
15 | \newcommand\startappendix{\appendix\appendixpage\addappheadtotoc} | |
16 | ||
17 | \newcommand\imgfig[4]{ | |
18 | \begin{figure}[h] | |
19 | \centering | |
20 | \includegraphics[scale=#2]{figures/#1} | |
21 | \caption{#3} | |
22 | \label{fig:#4} | |
23 | \end{figure}} | |
24 | ||
25 | \definecolor{gray}{gray}{0.95} | |
26 | ||
27 | \lstset{ | |
28 | language=Python, | |
29 | basicstyle=\footnotesize\ttfamily, | |
30 | numbersep=5pt, | |
31 | tabsize=2, | |
32 | extendedchars=true, | |
33 | breaklines=true, | |
34 | stringstyle=\ttfamily, | |
35 | showspaces=false, | |
36 | showtabs=false, | |
37 | xleftmargin=17pt, | |
38 | framexleftmargin=17pt, | |
39 | framexrightmargin=5pt, | |
40 | framexbottommargin=4pt, | |
41 | backgroundcolor=\color{gray}, | |
42 | showstringspaces=false | |
43 | } | |
44 | ||
1 | 45 | \title{Stable Flight and Object Tracking with a Quadricopter using an |
2 | 46 | Android Device} |
3 | 47 | \author{Benjamin Bardin \and William Brown |
4 | 48 | \and Dr. Paul Blaer} |
5 | 49 | |
6 | 50 | \begin{document} |
7 | \todolist | |
8 | 51 | |
9 | 52 | \maketitle |
10 | 53 | |
18 | 61 | platform on our first hardware iteration, named Jabberwock. |
19 | 62 | \end{abstract} |
20 | 63 | \tableofcontents |
64 | \pagebreak | |
21 | 65 | |
22 | 66 | \section{Motivation} |
23 | 67 | \subsection{Objectives} |
76 | 120 | |
77 | 121 | \section{Pilot Android Application} |
78 | 122 | \label{sec:pilot} |
79 | The Pilot program has two core functionalities. The first is the | |
80 | actual robotic control of the quadrocopter itself; the second is | |
81 | communication with the control server. The parallel processing | |
123 | The Pilot program has two core functionalities. The first is the | |
124 | actual robotic control of the quadrocopter itself; the second is | |
125 | communication with the control server. The parallel processing | |
82 | 126 | required in these distinct tasks is complicated by the performance |
83 | requirements of the program. Flight control processing must take place | |
84 | in real time -- or an extremely close approximation. Communication | |
127 | requirements of the program. Flight control processing must take place | |
128 | in real time -- or an extremely close approximation. Communication | |
85 | 129 | with the control server, on the other hand, yields priority to flight |
86 | control. Consequently, a great deal of effort went into prioritizing | |
87 | inter-thread communications and access of shared data. For flight | |
88 | control algorithms, blocking on locked data is unacceptable, since | |
89 | timely performance is essential; instead of blocking, they will use | |
90 | the most recent, locally stored version of the data requested. For | |
91 | communication algorithms, accessing flawed data is unacceptable; the | |
130 | control. Consequently, a great deal of effort went into prioritizing | |
131 | inter-thread communications and access of shared data. For flight | |
132 | control algorithms, blocking on locked data is unacceptable, since | |
133 | timely performance is essential; instead of blocking, they will use | |
134 | the most recent, locally stored version of the data requested. For | |
135 | communication algorithms, accessing flawed data is unacceptable; the | |
92 | 136 | control server must not receive out-of-date information portrayed as |
93 | 137 | current. |
94 | 138 | |
95 | 139 | \subsection{Flight Control} |
96 | Flight control itself is divided into two main components: navigation | |
140 | Flight control itself is divided into two main components: navigation | |
97 | 141 | and guidance. |
98 | 142 | |
99 | 143 | \subsubsection{Navigation} |
100 | Navigation determines a desired velocity vector for the quadrocopter. | |
101 | In “manual” mode, it simply accepts this vector from the control | |
102 | server. In “autopilot” mode, or when the connection is lost, autopilot | |
103 | subroutines determine the desired velocity vector. It's determination | |
104 | is based on two factors. The first is its current location. The second | |
105 | is either previously transmitted autopilot instructions, or | |
106 | pre-programmed safeties (for low power, bad network, etc.). | |
144 | Navigation determines a desired velocity vector for the quadrocopter. | |
145 | In ``manual'' mode, it simply accepts this vector from the control | |
146 | server. In ``autopilot'' mode, or when the connection is lost, autopilot | |
147 | subroutines determine the desired velocity vector. It's determination | |
148 | is based on two factors. The first is its current location. The second | |
149 | is either previously transmitted autopilot instructions, or | |
150 | pre-programmed safeties (for low power, bad network, etc.). | |
107 | 151 | |
108 | 152 | \subsubsection{Guidance} |
109 | Guidance takes the desired velocity from Navigation, and uses PID | |
153 | Guidance takes the desired velocity from Navigation, and uses PID | |
110 | 154 | loops to adjust individual motor speeds to achieve and maintain that |
111 | vector. To improve the performance of the PID loop, the system is | |
112 | transformed into an approximately linear one. The transformation | |
155 | vector. To improve the performance of the PID loop, the system is | |
156 | transformed into an approximately linear one. The transformation | |
113 | 157 | accounts both for the quadratic relationship between motor speed and |
114 | thrust, and for changing effects of motor thrust as its orientation | |
158 | thrust, and for changing effects of motor thrust as its orientation | |
115 | 159 | changes. |
116 | 160 | |
117 | 161 | \section{Server Software} |
201 | 245 | color from red to green, denoting the current speed of the motor. |
202 | 246 | |
203 | 247 | \section{Communication} |
204 | Communication is composed of two main components: telemetry and | |
205 | commands/data. Each is relayed on separate ports, since commands must | |
248 | Communication is composed of two main components: telemetry and | |
249 | commands/data. Each is relayed on separate ports, since commands must | |
206 | 250 | be relayed as synchronously as possible and telemetry will be |
207 | 251 | asynchronous. |
208 | 252 | |
209 | 253 | \subsection{Telemetry} |
210 | 254 | The telemetry modules continuously run the Android's preview |
211 | functionality, at 5fps. Each frame is saved to a buffer as it is | |
212 | available, overwriting the previous frame. When the Android has | |
213 | finished sending one frame to the control server, it immediately | |
214 | copies the buffer and starts sending the frame. The result is real | |
215 | time telepresence, at approximately two to four fps and a lag of | |
255 | functionality, at 5fps. Each frame is saved to a buffer as it is | |
256 | available, overwriting the previous frame. When the Android has | |
257 | finished sending one frame to the control server, it immediately | |
258 | copies the buffer and starts sending the frame. The result is real | |
259 | time telepresence, at approximately two to four fps and a lag of | |
216 | 260 | less than one second. |
217 | 261 | |
218 | 262 | \subsection{Commands and Data} |
219 | 263 | Commands and data are relayed in the form of strings over standard |
220 | Java sockets. When the connection is lost, the Android device | |
221 | immediately tries to reconnect, continuing to do so indefinitely. | |
222 | While the connection is lost, autopilot is enabled and the | |
223 | ``communication lost'' pre-programmed instruction set is engaged. | |
264 | Java sockets. When the connection is lost, the Android device | |
265 | immediately tries to reconnect, continuing to do so indefinitely. | |
266 | While the connection is lost, autopilot is enabled and the | |
267 | ``communication lost'' pre-programmed instruction set is engaged. | |
224 | 268 | |
225 | 269 | \subsection{Message Formats} |
226 | 270 | \label{sec:msgs} |
227 | 271 | Messages between the Android and the control server are sent as |
228 | strings, delimited by colons. The strings from the control server -- | |
229 | commands -- contain the instruction itself, prefixed by a sequence of | |
230 | meta-data describing the instruction. Similarly, data from the Android | |
231 | device contain not just the data, but also a prefix tag describing the | |
232 | data. This enables somewhat efficient analysis on each end: messages can | |
272 | strings, delimited by colons. The strings from the control server -- | |
273 | commands -- contain the instruction itself, prefixed by a sequence of | |
274 | meta-data describing the instruction. Similarly, data from the Android | |
275 | device contain not just the data, but also a prefix tag describing the | |
276 | data. This enables somewhat efficient analysis on each end: messages can | |
233 | 277 | be routed only to those components that are registered to process a |
234 | 278 | given prefix tag. Messages are not transmitted directly between the |
235 | Android device and the control server. Instead, they are routed through | |
236 | a separate, dedicated broker server. This enables the control server | |
237 | itself to operate easily from different IP addresses, and hence from | |
279 | Android device and the control server. Instead, they are routed through | |
280 | a separate, dedicated broker server. This enables the control server | |
281 | itself to operate easily from different IP addresses, and hence from | |
238 | 282 | various locations. It also allows for easy logging and playback of |
239 | 283 | sessions -- the broker server logs all data and commands, and can replay |
240 | 284 | a session later for analysis. |