Problem with motor control (Python)

[peolo]

Hello everyone, first post here, hope I don't screw it up

So I am currently writing a Python script for the Pybricks Exploration Rover: https://pybricks.com/projects/sets/mindstorms-robot-inventor/fan-inventions/exploration-rover/ I have decided to write the script myself to practice Python. I am however facing a couple of issues that can be seen below (it's a video that I have recorded):

So the issues are:

- The rover moves initially as expected (forward, all 4 motors correctly), but when I stop it, the motors jerk violently a little bit.

- When I start it again, only the front motors move.

- Sometimes, after stopping it, I hear this "whirring" like it's coming from the motors, but the script is not running. It's near the end of the video, you probably need to increase the volume to hear it.

It doesn't seem to be an hardware problem, because when I try to run the official Exploration Rover script, it runs as expected. So it's probably an issue with my code, which is posted below.

from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App
from mindstorms.control import wait_for_seconds, wait_until, Timer
from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to
import math

import time

hub = MSHub()

# Motors and sensors
motor_front = MotorPair('C','D')
motor_back = MotorPair('E','F')
motorC = Motor('C')
motorD = Motor('D')
motorE = Motor('E')
motorF = Motor('F')

distance_sensor = DistanceSensor('B')

################################################################################################################################################
# Coroutines code from https://github.com/arturomoncadatorres/lego-mindstorms/blob/main/base/charlie/programs/drum_solo.py
# (inspired also by https://gist.github.com/dlech/fa48f9b2a3a661c79c2c5880684b63ae)
# This code is needed to move the two motor pairs in parallel

# Useful Timer class

from utime import sleep as wait_for_seconds
from utime import ticks_diff, ticks_ms

# Mindstorms Timer doesn't allow decimal points, so we create our own timer
class Timer():
    """Replacement Timer class that allows decimal points so we can measure times of less than one second."""
    def __init__(self):
        self.start_ticks = 0

    def now(self):
        """Returns the time in seconds since the timer was last reset."""
        return ticks_diff(ticks_ms(), self.start_ticks) / 1000

    def reset(self):
        """Resets the timer."""
        self.start_ticks = ticks_ms()

timer_move = Timer()

################################################################################################################################################
# Code to move forward the rover

# Background timers
background_front_timer = Timer()
background_back_timer = Timer()

def move_forward():

    t_move = 2

    def background_front(seconds):
        """Coroutine
        Parameters
        ----------
        seconds (float): motor execution time
        """
        # This timer is used to measure how long it has been since we started
        background_front_timer.reset()

        # Here, we check if the execution time of this arm has exceeded
        # the desired duration (given by t).
        while background_front_timer.now() < seconds:
            # if it has not been long enought yet, then we "yield" to let the
            # rest of the program run for a while then we will come back here
            # later and check again
            yield

    def background_back(seconds): # Mirrors the coroutine above

        background_back_timer.reset()

        while background_back_timer.now() < seconds:
            yield

    def move_front():
        """Coroutine to execute the motion"""
        while True:
            # This is how we receive a parameter
            # In this case, it corresponds to the time the action should last
            t_action = yield

            # We make sure we only execute code if the received
            # value was transmitted correctly
            if not t_action == None:
                # We start moving the motor
                # Front wheels (C and D) need to be controlled at 1/2.5 times the speed of the rear wheels, given the gear ratio of the rear wheels
                # See also the Exploration Rover wordblock
                motor_front.start(0,20)

                # We check if its duration exceeds the maximum allowed
                yield from background_front(t_action)

                # We assume that the movement is immediate and takes no time
                # This isn't completely true, but for now it works

    def move_back(): # Mirrors the coroutine above
        while True:
            t_action = yield

            if not t_action == None:
                motor_back.start(0,-50)
                yield from background_back(t_action)

    # Since the move_left() and move_right() are coroutines and use yield
    # (i.e., they are not functions and thus have no return), they will NOT
    # run here when we call them. Instead, they will just be created as generator objects.
    # These generators will be used to run the functions one yield (i.e., step) at a time.
    front_generator = move_front()
    back_generator = move_back()

    # Now we will actually start the task
    # The task will be run as long as its
    # execution time is shorter than the allowed max duration (t_move).
    timer_move.reset()

    while timer_move.now() < t_move:

        next(front_generator)
        front_generator.send(t_move)

        next(back_generator)
        back_generator.send(t_move)

    return None

################################################################################################################################################
# Code to move backward the rover

# Background timers
background_front_timer = Timer()
background_back_timer = Timer()

def move_backward():

    t_move = 2

    def background_front(seconds):
        """Coroutine
        Parameters
        ----------
        seconds (float): motor execution time
        """
        # This timer is used to measure how long it has been since we started
        background_front_timer.reset()

        # Here, we check if the execution time of this arm has exceeded
        # the desired duration (given by t).
        while background_front_timer.now() < seconds:
            # if it has not been long enought yet, then we "yield" to let the
            # rest of the program run for a while then we will come back here
            # later and check again
            yield

    def background_back(seconds): # Mirrors the coroutine above

        background_back_timer.reset()

        while background_back_timer.now() < seconds:
            yield

    def move_front():
        """Coroutine to execute the motion"""
        while True:
            # This is how we receive a parameter
            # In this case, it corresponds to the time the action should last
            t_action = yield
            
            # We make sure we only execute code if the received
            # value was transmitted correctly
            if not t_action == None:
                # We start moving the motor
                # Front wheels (C and D) need to be controlled at 1/2.5 times the speed of the rear wheels, given the gear ratio of the rear wheels
                # See also the Exploration Rover wordblock
                motor_front.start(0,-20)

                # We check if its duration exceeds the maximum allowed
                yield from background_front(t_action)
                
                # We assume that the movement is immediate and takes no time
                # This isn't completely true, but for now it works

    def move_back(): # Mirrors the coroutine above
        while True:
            t_action = yield

            if not t_action == None:
                motor_back.start(0,50)
                yield from background_back(t_action)

    # Since the move_left() and move_right() are coroutines and use yield
    # (i.e., they are not functions and thus have no return), they will NOT
    # run here when we call them. Instead, they will just be created as generator objects.
    # These generators will be used to run the functions one yield (i.e., step) at a time.
    front_generator = move_front()
    back_generator = move_back()

    # Now we will actually start the task
    # The task will be run as long as its
    # execution time is shorter than the allowed max duration (t_move).
    timer_move.reset()
    
    while timer_move.now() < t_move:

        next(front_generator)
        front_generator.send(t_move)

        next(back_generator)
        back_generator.send(t_move)

    return None

################################################################################################################################################
# Code to stop the rover

def stop_driving():
    
    motor_front.stop()
    motor_back.stop()
    
    return None

################################################################################################################################################
# Code to change direction

def change_direction():

    # Move backward for 1 second
    t_end = time.time() + 1

    while time.time() < t_end:
        move_backward()

    stop_driving()

    # Rotate on the spot for 3 seconds
    t_end1 = time.time() + 3

    while time.time() < t_end1:
        motor_front.start(20)
        motor_back.start(-50)
        #motorC.start(20)
        #motorD.start(20)
        #motorE.start(-50)
        #motorF.start(-50)
    
    stop_driving()
    
    return None

################################################################################################################################################
# Main code

while True:
    move_forward()

    # If an obstacle is detected at less than 20 cm, it changes direction
    if distance_sensor.wait_for_distance_closer_than(20,'cm') == None:
        stop_driving()
        change_direction()

Do you know what it could be? Thanks a lot for any support!

Edited December 21, 2021 by peolo

[JopieK]

I don't know for sure if that is actually the issue here, but Python is actually not the best option for Embedded applications. C(++) or otherwise Rust would be a much more reliable choice. Also, after executing the if statement you perform stop_driving() and change_direction() but afterwards you do a move_forward() for a short time. It would be much better to turn it into a state machine or something. A quick solution would be to do the if but add an else and put move_forward() there.

[peolo]

14 hours ago, JopieK said:

I don't know for sure if that is actually the issue here, but Python is actually not the best option for Embedded applications. C(++) or otherwise Rust would be a much more reliable choice. Also, after executing the if statement you perform stop_driving() and change_direction() but afterwards you do a move_forward() for a short time. It would be much better to turn it into a state machine or something. A quick solution would be to do the if but add an else and put move_forward() there.

Thanks for the answer. I am honestly not familiar with C, I saw the option to use Python and I tried it.

I will look into state machines.

[JopieK]

I would stick with Python for now then. While from a technical perspective C(++) is a better choice for more complicated microcontroller/robotics programs, from an educational standpoint learning Python is much easier. I also use it quite a lot but for data processing etc.

Hope this should already work well for you. States are pretty much an optimization (to make the code more flexible etc.).

 

################################################################################################################################################
# Main code

while True:
    # If an obstacle is detected at less than 20 cm, it changes direction
    if distance_sensor.wait_for_distance_closer_than(20,'cm') == None:
        stop_driving()
        change_direction()
    else:
    	move_forward()

 

[peolo]

Unfortunately adding the else didn't solve the issue... I am trying to understand why. There seem to be issues with the turning on the spot function.