Trouble with timing multitasked motors

[Jormono1]

Hey all, I am working on a pen plotter run by a technic hub and 3 motors. The issue I am coming into right now is that if I multitask 2 motors (x and y to make an angled line) I'm finding that whichever motor has the least distance to travel will run to its target then stop while the other is still moving which results in something of a "bent line". So for ease of explanation, lets say the coordinate has a longer X travel than it does a Y travel. So the if I run the X and the Y motors at the same speed the Y motor hits target first and the X motor keeps going until it reaches its own target. I wrote some script to evaluate the travel distance for both motors, scaling down the speed for Y motor so it reaches its target at the same time as the X motor. I then realized it very much needed a minimum speed. I wrote a separate program to simply run each motor at reducing speeds until I found a minimum that it would run smoothly. So I put in the minimum speeds (different per motor for some reason) and if the calculated speed comes out lower than minimum, it then sets the minimum for that motor, and then evaluates an increased speed for the longer travel to sync the timing again.

This doesn't seem to work. I'm either getting right angles or bent lines and I have no idea why. See below for my code, I removed 99% of the coordinate points for brevity, its 200 lines of data that I've auto generated with a python script on my computer. See below for a picture of what I'm getting out of the machine with some red lines to indicate a rough idea of where the angled lines should be where instead I'm getting 2 segement lines.

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Direction, Port, Side, Stop
from pybricks.tools import multitask, run_task

hub = TechnicHub()
X_Motor = Motor(Port.A)
Y_Motor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
Z_Motor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
move_speed = 300
prev_x = 0
prev_y = 0

### Coordinates are in [X, Y, Z] format ###
### False means no change, ignore ###
### Values are multiplied by 10 and rounded to make them whole numbers ###
### Float values would take up substantially more memory which is fairly limited in the hub ###
Coordinates = [[71, 1125, False],
[False, False, 1],
[1901, 68, False],
[False, 64, False],
[1653, 1830, False],
[0, 0, 46]]

async def homing():
    Z_Motor.run_until_stalled(move_speed,duty_limit=20)  # raise pen
    print("Homing X Axis")
    X_Motor.run_until_stalled(-move_speed, duty_limit=20)    # run until 0
    X_Motor.reset_angle(0)
    print("Homing Y Axis")
    Y_Motor.run_until_stalled(-move_speed, duty_limit=20)
    Y_Motor.reset_angle(0)

async def move (motor, coord, speed=move_speed):
    await motor.run_target(speed, coord, wait=True)

### if you run both motors at the same speed over different distances it will not result in a straight line ###
### instead run motor with less distance to travel proportionately slower so they take the same overall time and result in a straight line ###
async def xy_move (x, y):
    x_travel = abs(prev_x - x)
    y_travel = abs(prev_y - y)
    if x_travel > y_travel:
        x_time = x_travel/move_speed # degrees movement / degrees per second speed = seconds of travel
        y_speed = y_travel/x_time    # travel distance / seconds of travel = degrees per second speed
        if y_speed < 100:           # handles minimum speed for Y motor
            y_time = y_travel/100
            x_speed = x_travel/y_time
            speed = [x_speed, 100]
        else:
            speed = [move_speed, y_speed]
    elif y_travel > x_travel:           # if Y has to move further than X
        y_time = y_travel/move_speed   # Y will take this time to travel the required distance at default speed
        x_speed = x_travel/y_time      # x will need to travel this reduced speed to keep pace with y
        if x_speed < 75:               # if speed below given minimum, motor doesn't seem to work correctly
            x_time = x_travel/75       # scales up speed for longer segement and scales down speed for shorter segment to min
            y_speed = y_travel/x_time
            speed = [75, y_speed]
        else:
            speed = [x_speed, move_speed]
    elif y_travel == x_travel:         # in the off chance they do travel same distance (45 degree angle) they can run the same speed
        speed = [move_speed, move_speed]
    await multitask(move(X_Motor, x, speed[0]), move(Y_Motor, y, speed[1]))

async def main():
    for line in Coordinates:
        X = line[0]
        Y = line[1]
        Z = line[2]

        if (X != False) and (Y != False):
            print("Moving to: " + str(X/10) + ", " + str(Y/10))
            await xy_move(float(X/10), float(Y/10))
            prev_x = X/10
            prev_y = Y/10

        elif (X != False) and (Y == False):
            print("Moving to: " + str(X/10))
            await move(X_Motor, float(X/10))
            prev_x = X/10

        elif (Y != False) and (X == False):
            print("Moving to: " + str(Y/10))
            await move(Y_Motor, float(Y/10))
            prev_y = Y/10

        elif Z != False:
            if Z == 46: # 0 and 45 were originally target angles, but some modifications to my build reduced the limit of travel, values are now arbitrary
                print("Pen Up")
                await Z_Motor.run_until_stalled(move_speed, duty_limit=15)
            elif Z == 1:
                print("Pen Down")
                await Z_Motor.run_until_stalled(-move_speed, duty_limit=15)
    print("all done")

run_task(homing())
run_task(main())

[dlech]

It is really hard to control the motors at speeds <= 100 degrees per second. If you can add gears or something like that so that the motors are operating between 400 and 800 degrees per second, it might work better.

TIP: you can add the gear rations to the motor setup (i.e. X_Motor = Motor(Port.A)) then you don't have to change the rest of your program. The speeds in your program will then correspond to the speed at the end of the gear train instead of the speed of the motor itself.

Also, the run_until_stalled() lines are missing await, but that isn't related to the issue.

[Jormono1]

I geared down the speed yesterday, I now have 12:40 and 16:40 based on what would physically fit. This does allow me to run the motors at much higher speeds as you suggested. I also fixed the run until stalled lines in my homing function, thanks for catching that! I also tried "telling Pybricks" about the gearing, but it reduced the overall degrees of travel by ~half which gave me less definition. I also already have a python script which I use to generate the files which has a function for converting mm to degrees of rotation so I simply updated the data for that.

My image is a little stretched from the gearing being different on x from y, I'll deal/worry about that later. Adjusting the default speed did not seem to have any effect on the issue of drawing angled lines

[laurensvalk]

When you multitask two motor.run_target commands, they are always going to have different durations and acceleration times and they are not synchronized.

What about the following approach?

while True:
     x, y = position_as_a_function_of_time(time)
     motor_x.track_target(x)
     motor_y.track_target(y)

track_target doesn't ramp up or down the speed, but goes to your position as fast as possible. Instead, you can gradually vary x and y over time to achieve the desired speed or acceleration.

If you let x and y increase linearly with time, you'd get a straight line, for example.

[Jormono1]

I must have either misunderstood your suggestion or have a problem in my code (I actually know there is a problem because I keep getting a divide by zero error for some reason on x_segment = x_travel/segments)

async def xy_move (x, y):
    xy_travel = sqrt(pow(x-prev_x,2) + pow(y-prev_y,2))
    x_travel = abs(prev_x - x)
    y_travel = abs(prev_y - y)
    segments = xy_travel/16  # 16 degrees is approximation of 1 mm, yields number of ~1mm segments along line
    x_segment = x_travel/segments     # x travel distance broken up into chunks of ~1 mm line
    y_segment = y_travel/segments
    mid_x = prev_x
    mid_y = prev_y

    if prev_x > x:  # check direction of travel for add or subtract
        x_heading = -1  # value is a multiplier for x_segment value
    elif prev_x < x:
        x_heading = 1
        
    if prev_y > y:
        y_heading = -1
    elif prev_y < y:
        y_heading = 1
    
    for segment in range(segments):
        mid_x = mid_x + (x_heading * x_segment)
        mid_y = mid_y + (y_heading * y_segment)
        X_Motor.track_target(mid_x)
        Y_Motor.track_target(mid_y)

[laurensvalk]

The suggestion is to move your motors as a function of time, not break them into segments.

So the target to track could be: x_target = x_starting_point + x_speed * time.

[Jormono1]

@laurensvalk , sorry for the very delayed response, I've been working on this here and there as time allows but most of my time has been spent on home renovations which sadly take precedent over my project.

I've not been able to figure out how to do what you suggest, I think I understand the concept but for some reason I'm hitting an absolute brick wall when I try to put it into code.

In my experimentation I've found that the track_target function doesn't appear to respond at all to setting a control limit speed, it always travels at the same speed no matter what max speed I set. Is this correct behavior? It does seem the intention of the track_target function to move at max speed, I just would have assumed that setting a control limit to a lower speed would reduce the effective speed of the function.

I've been playing around with timing my motors more carefully but I'm struggling to keep the speeds within an acceptable range (less than ~100 deg/s and the movement is jittery, over 2,000 deg/s the motor will cap out no matter what I tell it to do. I have this dumbed down XY only program with some dummy coordinates plugged in

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Direction, Port, Side, Stop
from pybricks.tools import multitask, run_task
from umath import sqrt

hub = TechnicHub()
X_Motor = Motor(Port.A)
Y_Motor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
Z_Motor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
limit = 20
move_speed = 900    # deg / s
min_speed = 100
max_speed = 2000
prev_x = 1  # deg
prev_y = 1

Coordinates = [[249, 2526, False],
[6378, 163, False],
[6803, 419, False],
[7205, 664, False],
[8020, 1138, False],
[10289, 2313, False],
[10537, 4073, False],
[7205, 664, False]]

async def homing():
    Z_Motor.run_until_stalled(move_speed,duty_limit=limit)                            # raise pen
    print("Homing X Axis")
    X_Motor.run_until_stalled(-move_speed, duty_limit=limit)    # run until 0
    X_Motor.reset_angle(0)
    print("Homing Y Axis")  
    Y_Motor.run_until_stalled(-move_speed, duty_limit=limit)
    Y_Motor.reset_angle(0)

async def main():       # Main loop, read coordinates and call movement functions 
    for line in Coordinates:
        X = float(line[0]/10)                                           # coordinates are stored as integers, divide by 10 to get actual value
        Y = float(line[1]/10)

        x_travel = abs(prev_x - X)                                      # net travel x
        y_travel = abs(prev_y - Y)                                      # net travel y
        xy_travel = sqrt(pow(x_travel,2) + pow(y_travel,2))             # net travel xy
        print("X = ", str(X), ", Y = ", str(Y))                       
        print("XY travel = ", str(xy_travel))                           
        timex = x_travel / move_speed                                   # deg / deg/s = s
        timey = y_travel / move_speed
        travel_time = max(timex, timey)
        speedx = x_travel / travel_time                                 # deg / s = deg/s
        speedy = y_travel / travel_time
        print("x speed: ", str(speedx), "y speed: ", str(speedy))

        if min(speedx, speedy) < 100:
            if speedx < speedy:
                speedx = min_speed
                timex = x_travel / speedx
                speedy = y_travel / timex
            elif speedy < speedx:
                speedy = min_speed
                timey = y_travel / speedy
                speedx = x_travel / timey
            print("Corrected Speeds:    X: ", str(speedx),", Y: ", str(speedy))
            if max(speedx, speedy) > max_speed:
                if speedx > speedy:
                    speedx = max_speed
                    timex = x_travel / speedx
                    speedy = y_travel / timex
                elif speedy > speedx:
                    speedy = max_speed
                    timey = y_travel / speedy
                    speedx = x_travel / timey
                print("Re-Corrected Speeds:    X: ", str(speedx),", Y: ", str(speedy))
        speedx = int(round(speedx))
        speedy = int(round(speedy))
        print("~~~")
        await multitask(X_Motor.run_target(speedx, X, wait=True), Y_Motor.run_target(speedy, Y, wait=True))

run_task(homing())  
run_task(main())
run_task(homing())

The following is the output of my print statements, I'm getting a lot of jittery movement and not even close to synchronized movements

Homing X Axis
Homing Y Axis
X =  24.9 , Y =  252.6
XY travel =  252.7326
x speed:  85.49284 y speed:  900.0
Corrected Speeds:    X:  100 , Y:  1052.72
~~~
X =  637.8 , Y =  16.3
XY travel =  636.9838
x speed:  900.0 y speed:  21.62374
Corrected Speeds:    X:  4162.091 , Y:  100
Re-Corrected Speeds:    X:  2000 , Y:  48.05276
~~~
X =  680.3 , Y =  41.9
XY travel =  680.5302
x speed:  900.0 y speed:  54.18814
Corrected Speeds:    X:  1660.88 , Y:  100
~~~
X =  720.5 , Y =  66.4
XY travel =  722.4662
x speed:  900.0 y speed:  81.80682
Corrected Speeds:    X:  1100.153 , Y:  100
~~~
X =  802.0 , Y =  113.8
XY travel =  808.9034
x speed:  900.0 y speed:  126.7416
~~~
X =  1028.9 , Y =  231.3
XY travel =  1053.383
x speed:  899.9999 y speed:  201.6441
~~~
X =  1053.7 , Y =  407.3
XY travel =  1128.387
x speed:  900.0 y speed:  347.3639
~~~
X =  720.5 , Y =  66.4
XY travel =  722.4662
x speed:  900.0 y speed:  81.80682
Corrected Speeds:    X:  1100.153 , Y:  100
~~~
Homing X Axis
Homing Y Axis

And below is the code I run when I want to test the machine at different speeds

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

hub = TechnicHub()
X_Motor = Motor(Port.A)
Y_Motor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
Z_Motor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
set_speed1 = 500
set_speed2 = 200
set_speed3 = 100
x_limits = X_Motor.control.limits()
print("Default speed = " + str(x_limits[0]))
print("Default acceleration = " + str(x_limits[1]))
print("Default torque = " + str(x_limits[2]))

def movement_loop(speed):
    X_Motor.track_target(2000)
    while not X_Motor.done():
        wait(1)
    Y_Motor.track_target(2000)
    while not Y_Motor.done():
        wait(1)
    X_Motor.run_target(speed, 200, wait=True)
    Y_Motor.run_target(speed, 200, wait=True)

def homing():
    print("Homing")
    X_Motor.run_until_stalled(-set_speed3, duty_limit=15)    # run until 0
    X_Motor.reset_angle(0)
    Y_Motor.run_until_stalled(-set_speed3, duty_limit=15)
    Y_Motor.reset_angle(0)

def main():
    homing()
    
    X_Motor.control.limits(speed=set_speed1)
    Y_Motor.control.limits(speed=set_speed1)
    print("Motor speed set to ", str(set_speed1))
    movement_loop(set_speed1)

    X_Motor.control.limits(speed=set_speed2)
    Y_Motor.control.limits(speed=set_speed2)
    print("Motor speed set to ", str(set_speed2))
    movement_loop(set_speed2)

    X_Motor.control.limits(speed=set_speed3)
    Y_Motor.control.limits(speed=set_speed3)
    print("Motor speed set to ", str(set_speed3))
    movement_loop(set_speed3)

    homing()

main()