Merge pull request #7 from RaspberryPiFoundation/n-rotations

Allow rotating of single or multiple motors by specifying number of rotations, rather than degrees

Author: grega

src/motor.c

@@ -1152,6 +1152,66 @@ Motor_run_for_degrees(PyObject *self, PyObject *args, PyObject *kwds)
 }
 
 
+static PyObject *
+Motor_run_for_rotations(PyObject *self, PyObject *args, PyObject *kwds)
+{
+    MotorObject *motor = (MotorObject *)self;
+    static char *kwlist[] = {
+        "rotations", "speed", "max_power", "stop",
+        "acceleration", "deceleration", "stall",
+        "blocking", NULL
+    };
+    int32_t rotations;
+    int32_t speed;
+    uint32_t power = motor->default_max_power;
+    uint32_t accel = motor->default_acceleration;
+    uint32_t decel = motor->default_deceleration;
+    int stall = motor->default_stall;
+    uint32_t stop = MOTOR_STOP_USE_DEFAULT;
+    uint8_t use_profile = 0;
+    int parsed_stop;
+    int blocking = 1;
+
+    if (motor->is_detached)
+    {
+        PyErr_SetString(cmd_get_exception(), "Motor is detached");
+        return NULL;
+    }
+
+    if (PyArg_ParseTupleAndKeywords(args, kwds,
+                                    "ii|IIIIpp:run_for_rotations", kwlist,
+                                    &rotations, &speed, &power, &stop,
+                                    &accel, &decel, &stall, &blocking) == 0)
+        return NULL;
+
+    if (device_ensure_mode_info(motor->device) < 0)
+        return NULL;
+
+    speed = CLIP(speed, SPEED_MIN, SPEED_MAX);
+    power = CLIP(power, POWER_MIN, POWER_MAX);
+    accel = CLIP(accel, ACCEL_MIN, ACCEL_MAX);
+    decel = CLIP(decel, DECEL_MIN, DECEL_MAX);
+    if ((parsed_stop = parse_stop(motor, stop)) < 0)
+    {
+        PyErr_Format(PyExc_ValueError, "Invalid stop state: %d", stop);
+        return NULL;
+    }
+
+    if (set_acceleration(motor, accel, &use_profile) < 0 ||
+        set_deceleration(motor, decel, &use_profile) < 0 ||
+        set_stall(motor, stall) < 0)
+        return NULL;
+
+    if (cmd_start_speed_for_degrees(port_get_id(motor->port),
+                                    rotations * 360, speed, power,
+                                    (uint8_t)parsed_stop, use_profile,
+                                    blocking) < 0)
+        return NULL;
+
+    Py_RETURN_NONE;
+}
+
+
 static PyObject *
 Motor_run_to_position(PyObject *self, PyObject *args, PyObject *kwds)
 {
@@ -1352,6 +1412,11 @@ static PyMethodDef Motor_methods[] = {
         METH_VARARGS | METH_KEYWORDS,
         "Run the motor for the given angle"
     },
+    {
+        "run_for_rotations", (PyCFunction)Motor_run_for_rotations,
+        METH_VARARGS | METH_KEYWORDS,
+        "Run the motor for rotations"
+    },
     {
         "run_to_position", (PyCFunction)Motor_run_to_position,
         METH_VARARGS | METH_KEYWORDS,

src/pair.c

@@ -903,6 +903,61 @@ MotorPair_run_for_degrees(PyObject *self, PyObject *args, PyObject *kwds)
     Py_RETURN_NONE;
 }
 
+static PyObject *
+MotorPair_run_for_rotations(PyObject *self, PyObject *args, PyObject *kwds)
+{
+    MotorPairObject *pair = (MotorPairObject *)self;
+    static char *kwlist[] = {
+        "rotations", "speed0", "speed1", "max_power", "stop",
+        "acceleration", "deceleration", "blocking",
+        NULL
+    };
+    int32_t rotations;
+    int32_t speed0, speed1;
+    uint32_t power = 100;
+    uint32_t accel = pair->default_acceleration;
+    uint32_t decel = pair->default_deceleration;
+    uint32_t stop = MOTOR_STOP_USE_DEFAULT;
+    uint8_t use_profile = 0;
+    int parsed_stop;
+    int blocking = 1;
+
+    if (!PyArg_ParseTupleAndKeywords(args, kwds,
+                                     "iii|IIIIp:run_for_rotations", kwlist,
+                                     &rotations, &speed0, &speed1,
+                                     &power, &stop,
+                                     &accel, &decel, &blocking))
+        return NULL;
+
+    /* If the object is invalid, return False */
+    if (pair->id == INVALID_ID)
+        Py_RETURN_FALSE;
+
+    speed0 = CLIP(speed0, SPEED_MIN, SPEED_MAX);
+    speed1 = CLIP(speed1, SPEED_MIN, SPEED_MAX);
+    power = CLIP(power, POWER_MIN, POWER_MAX);
+    accel = CLIP(accel, ACCEL_MIN, ACCEL_MAX);
+    decel = CLIP(decel, DECEL_MIN, DECEL_MAX);
+
+    if ((parsed_stop = parse_stop(stop)) < 0)
+    {
+        PyErr_Format(PyExc_ValueError, "Invalid stop state: %d", stop);
+        return NULL;
+    }
+
+    if (set_acceleration(pair, accel, &use_profile) < 0 ||
+        set_deceleration(pair, decel, &use_profile) < 0)
+        return NULL;
+
+    if (cmd_start_speed_for_degrees_pair(pair->id, rotations * 360,
+                                         speed0, speed1, power,
+                                         (uint8_t)parsed_stop,
+                                         use_profile, blocking) < 0)
+        return NULL;
+
+    Py_RETURN_NONE;
+}
+
 
 static PyObject *
 MotorPair_run_to_position(PyObject *self, PyObject *args, PyObject *kwds)
@@ -1023,6 +1078,11 @@ static PyMethodDef MotorPair_methods[] = {
         METH_VARARGS | METH_KEYWORDS,
         "Run the motor pair for the given angle"
     },
+    {
+        "run_for_rotations", (PyCFunction)MotorPair_run_for_rotations,
+        METH_VARARGS | METH_KEYWORDS,
+        "Run the motor pair for N rotations"
+    },
     {
         "run_to_position", (PyCFunction)MotorPair_run_to_position,
         METH_VARARGS | METH_KEYWORDS,