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 | |