motorcontrol: simplify gear ratio computation
fw size -48
@@ -119,33 +119,19 @@ EncodedMotor
Arguments:
port {const} -- Port to which the device is connected: PORT_A, PORT_B, etc.
direction {const} -- DIR_NORMAL or DIR_INVERTED (default: {DIR_NORMAL})
- gear_teeth {list} -- Description
+ first_gear {int} -- Number of teeth of gear attached to motor (default: {None})
+ last_gear {int} -- Number of teeth of last gear in the gear train (default: {None})
"""
*/
mp_obj_t motor_EncodedMotor_make_new(const mp_obj_type_id_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args){
- mp_arg_check_num(n_args, n_kw, 1, 3, false);
+ mp_arg_check_num(n_args, n_kw, 1, 4, false);
motor_EncodedMotor_obj_t *self = m_new_obj(motor_EncodedMotor_obj_t);
self->base.type = (mp_obj_type_t*) type;
self->port = mp_obj_get_int(args[0]);
int8_t direction = (n_args > 1) ? mp_obj_get_int(args[1]) : PBIO_MOTOR_DIR_NORMAL;
- // Determine gear ratio from number of teeth on each gear
- float_t gear_ratio = 1;
- if (n_args >= 3) {
-
- mp_obj_t *teeth;
- size_t number_of_gears;
- mp_obj_get_array(args[2], &number_of_gears, &teeth);
- if (number_of_gears < 2) {
- pb_raise_pbio_error(PBIO_ERROR_INVALID_ARG);
- }
- float_t teeth_now = mp_obj_get_int(teeth[0]);
- for (int16_t gear = 1; gear <= number_of_gears-1; gear++) {
- float_t teeth_next = mp_obj_get_int(teeth[gear]);
- gear_ratio = gear_ratio * (teeth_next/teeth_now);
- teeth_now = teeth_next;
- }
- }
- pbio_error_t err = pbio_encmotor_setup(self->port, type->device_id, direction, gear_ratio);
+ int16_t teeth_first = (n_args == 4) ? mp_obj_get_int(args[2]) : 1;
+ int16_t teeth_last = (n_args == 4) ? mp_obj_get_int(args[3]) : 1;
+ pbio_error_t err = pbio_encmotor_setup(self->port, type->device_id, direction, ((float_t) teeth_last)/teeth_first); // TODO: Don't compute gear ratio here. Pass on teeth numbers to motorcontrol to save some more computations there
pb_raise_pbio_error(err);
return MP_OBJ_FROM_PTR(self);
}
