Guidance accpets manual motor vectors
Ben
11 years ago
Binary diff not shown
Binary diff not shown
37 | 37 | |
38 | 38 | /** Message for Guidance handler, passing a new PID value **/ |
39 | 39 | 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; | |
40 | 43 | |
41 | 44 | /* Sensor indices */ |
42 | 45 |
9 | 9 | * |
10 | 10 | * May receive the following messages from Chopper components: |
11 | 11 | * <pre> |
12 | * GUID:PID:<pid_loop_number>:<pid_parameter_index>:<pid_parameter_value> | |
12 | * GUID: | |
13 | * PID:<pid_loop_number>:<pid_parameter_index>:<pid_parameter_value> | |
14 | * AUTOMATIC | |
15 | * VECTOR:<north_motor_speed>:<south_motor_speed>:<east_motor_speed>:<west_motor_speed> | |
16 | * | |
13 | 17 | * </pre> |
14 | 18 | * |
15 | 19 | * @author Benjamin Bardin |
26 | 30 | public static final double MAX_ANGLE = 20; |
27 | 31 | |
28 | 32 | /** 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; | |
30 | 34 | |
31 | 35 | /** Tag for logging */ |
32 | 36 | public static final String TAG = new String("chopper.Guidance"); |
138 | 142 | case NEW_PID_VALUE: |
139 | 143 | mGain[msg.arg1][msg.arg2] = (Double)msg.obj; |
140 | 144 | 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; | |
141 | 152 | } |
142 | 153 | } |
143 | 154 | }; |
166 | 177 | new Integer(parts[2]), |
167 | 178 | new Integer(parts[3]), |
168 | 179 | 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); | |
169 | 193 | newValue.sendToTarget(); |
170 | 194 | } |
171 | 195 | } |
365 | 389 | //Linearizes system with regard to prop thrust, not prop RPMs |
366 | 390 | mMotorSpeed[i] = Math.sqrt(mMotorSpeed[i]); |
367 | 391 | |
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 | ||
374 | 395 | |
375 | 396 | //Send motor values to motors here: |
397 | updateMotors(); | |
376 | 398 | |
377 | 399 | Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]); |
378 | 400 | //Sleep a while |
382 | 404 | else |
383 | 405 | mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED); |
384 | 406 | } |
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 | } | |
385 | 415 | } |
386 | 416 |