git.haldean.org droidcopter / ed201fe
Guidance accpets manual motor vectors Ben 11 years ago
4 changed file(s) with 41 addition(s) and 8 deletion(s). Raw diff Collapse all Expand all
Binary diff not shown
Binary diff not shown
3737
3838 /** Message for Guidance handler, passing a new PID value **/
3939 public static final int NEW_PID_VALUE = 109;
40
41 /** Message for Guidance handler, passing a manual vector for motorspeeds **/
42 public static final int NEW_GUID_VECTOR = 110;
4043
4144 /* Sensor indices */
4245
99 *
1010 * May receive the following messages from Chopper components:
1111 * <pre>
12 * GUID:PID:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
12 * GUID:
13 * PID:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
14 * AUTOMATIC
15 * VECTOR:&lt;north_motor_speed&gt;:&lt;south_motor_speed&gt;:&lt;east_motor_speed&gt;:&lt;west_motor_speed&gt;
16 *
1317 * </pre>
1418 *
1519 * @author Benjamin Bardin
2630 public static final double MAX_ANGLE = 20;
2731
2832 /** The maximum change in motor speed permitted at one time. Must be positive. */
29 public static final double MAX_DMOTOR = .1F;
33 public static final double MAX_DMOTOR = .05F;
3034
3135 /** Tag for logging */
3236 public static final String TAG = new String("chopper.Guidance");
138142 case NEW_PID_VALUE:
139143 mGain[msg.arg1][msg.arg2] = (Double)msg.obj;
140144 break;
145 case NEW_GUID_VECTOR:
146 Double[] mVector = (Double[])msg.obj;
147 for (int i = 0; i < 4; i++) {
148 mMotorSpeed[i] = mVector[i];
149 updateMotors();
150 }
151 break;
141152 }
142153 }
143154 };
166177 new Integer(parts[2]),
167178 new Integer(parts[3]),
168179 new Double(parts[4]));
180 newValue.sendToTarget();
181 }
182 if (parts[1].equals("AUTOMATIC")) {
183 mHandler.removeMessages(EVAL_MOTOR_SPEED);
184 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
185 }
186 if (parts[1].equals("VECTOR")) {
187 mHandler.removeMessages(EVAL_MOTOR_SPEED);
188 Double[] myVector = new Double[4];
189 for (int i = 0; i < 4; i++) {
190 myVector[i] = new Double(parts[i + 2]);
191 }
192 Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector);
169193 newValue.sendToTarget();
170194 }
171195 }
365389 //Linearizes system with regard to prop thrust, not prop RPMs
366390 mMotorSpeed[i] = Math.sqrt(mMotorSpeed[i]);
367391
368 mTempMotorSpeed[i] = mMotorSpeed[i];
369
370 }
371
372 //Pass filtered values to ChopperStatus.
373 mStatus.setMotorFields(mMotorSpeed);
392 mTempMotorSpeed[i] = mMotorSpeed[i];
393 }
394
374395
375396 //Send motor values to motors here:
397 updateMotors();
376398
377399 Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]);
378400 //Sleep a while
382404 else
383405 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
384406 }
407
408 /* To be finished */
409 private void updateMotors() {
410 //Pass filtered values to ChopperStatus.
411 mStatus.setMotorFields(mMotorSpeed);
412
413 //Pass motor values to motor controller!
414 }
385415 }
386416