git.haldean.org droidcopter / 12f0531
Fixed nav docs; added get-PID functionality to Guidance Ben 11 years ago
5 changed file(s) with 36 addition(s) and 9 deletion(s). Raw diff Collapse all Expand all
Binary diff not shown
Binary diff not shown
4040
4141 /** Message for Guidance handler, passing a manual vector for motorspeeds **/
4242 public static final int NEW_GUID_VECTOR = 110;
43
44 /** Message for Guidance handler, instructing it to obtain PID values **/
45 public static final int GET_PIDS = 11;
4346
4447 /* Sensor indices */
4548
1313 * May send the following messages to registered Receivables:<br>
1414 * <pre>
1515 * GUID:ERROR:&lt;loop_1_error&gt;:&lt;loop_2_error&gt;:&lt;loop_3_error&gt;:&lt;loop_4_error&gt;
16 * GUID:PID:VALUE:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
1617 * </pre>
1718 *
1819 * May receive the following messages from Chopper components:
1920 * <pre>
2021 * GUID:
21 * PID:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
22 * PID:
23 * SET:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
24 * GET
2225 * AUTOMATIC
2326 * VECTOR:&lt;north_motor_speed&gt;:&lt;south_motor_speed&gt;:&lt;east_motor_speed&gt;:&lt;west_motor_speed&gt;
2427 *
172175 updateMotors();
173176 }
174177 break;
178 case GET_PIDS:
179 Receivable source = (Receivable) msg.obj;
180
181 //Send each PID value to the requesting object
182 for (int i = 0; i < 4; i++) {
183 for (int j = 0; j < 3; j++) {
184 source.receiveMessage("GUID:PID:VALUE:" +
185 i + ":" + j + ":" +
186 mGain[i][j],
187 null);
188
189 }
190 }
191
192 break;
175193 }
176194 }
177195 };
195213 String[] parts = msg.split(":");
196214 if (parts[0].equals("GUID")) {
197215 if (parts[1].equals("PID")) {
198 Message newValue = Message.obtain(mHandler,
199 NEW_PID_VALUE,
200 new Integer(parts[2]),
201 new Integer(parts[3]),
202 new Double(parts[4]));
203 newValue.sendToTarget();
216 if (parts[2].equals("SET")) {
217 Message newValue = Message.obtain(mHandler,
218 NEW_PID_VALUE,
219 new Integer(parts[3]),
220 new Integer(parts[4]),
221 new Double(parts[5]));
222 newValue.sendToTarget();
223 }
224 if (parts[2].equals("GET")) {
225 Message getPids = Message.obtain(mHandler, GET_PIDS, source);
226 getPids.sendToTarget();
227 }
204228 }
205229 if (parts[1].equals("AUTOMATIC")) {
206230 mHandler.removeMessages(EVAL_MOTOR_SPEED);
2626 * <pre>
2727 * NAV:
2828 * SET:
29 * MANUAL
29 * MANUAL:&lt;north_south_vel&gt;:&lt;east_west_vel&gt;:&lt;up_down_vel&gt;:&lt;orientation&gt;
3030 * AUTOPILOT
3131 * AUTOTASK:&lt;travel_plan_index&gt;:&lt;serialized_NavTask&gt;
3232 * GET:AUTOTASKS
234234 autoPilot(false);
235235 double[] newTarget = new double[4];
236236 for (int i = 0; i < 4; i++) {
237 newTarget[i] = new Double(parts[i] + 3);
237 newTarget[i] = new Double(parts[i + 3]);
238238 }
239239 setTarget(newTarget);
240240 }