From 79a33473d4b25aa1bb2cc62c20d0b0ab8ff717f7 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 14 Dec 2020 11:18:08 +0100 Subject: [PATCH] pbio/servo: only reset used servos After a program completed we were resetting all hbridges for simplicity, but this was also turning off sensors that need power, like the Powered Up Color Sensor. This ensures that we only reset those motors for which we have a dc motor reference, which could only be initialized if there was a tacho. We will still need a mechanism for resetting dc motors. --- lib/pbio/src/servo.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 615cd3d27..09e4a1376 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -24,17 +24,18 @@ pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix return PBIO_ERROR_INVALID_OP; } - // Get, coast, and configure dc motor - err = pbio_dcmotor_get(srv->port, &srv->dcmotor, direction, true); + // Get and reset tacho + err = pbio_tacho_get(srv->port, &srv->tacho, direction, gear_ratio); if (err != PBIO_SUCCESS) { return err; } - // Get and reset tacho - err = pbio_tacho_get(srv->port, &srv->tacho, direction, gear_ratio); + // Get, coast, and configure dc motor + err = pbio_dcmotor_get(srv->port, &srv->dcmotor, direction, true); if (err != PBIO_SUCCESS) { return err; } + // Reset state pbio_control_stop(&srv->control); @@ -255,12 +256,11 @@ pbio_error_t pbio_servo_stop_force(pbio_servo_t *srv) { // Release claim from drivebases or other classes srv->claimed = false; - // Try to stop / coast motor whether or not initialized already + // Try to stop / coast motor if (srv->dcmotor) { return pbio_dcmotor_coast(srv->dcmotor); - } else { - return pbdrv_motor_coast(srv->port); } + return PBIO_SUCCESS; } pbio_error_t pbio_servo_run(pbio_servo_t *srv, int32_t speed) {