Remote control for 42114 Volvo Articulated Hauler

[repkovsky]

Update: It's published here

I managed to write a program for controlling 42114 Volvo Articulated Hauler with Bluetooth remote 88010, if someone is interested, the code is available here: https://github.com/repkovsky/technic/tree/main/42114-volvo-articulated-hauler. Both automatic and manual gearbox are implemented and they work rather well. In contrast to most of the remote control programs I decided to use classes, since without this it could be hard to write it in a clean way. There are many comments, so I hope this code still has some educational value. I would appreciate any comments, corrections as well as sharing it further. Here you can find little demo of driving, automated and manual gear switching and operating dumper https://www.youtube.com/watch?v=j4ZxOhXygNY.

[Gcopper1984]

Hi,
very cool job and very great python example! I will give it a try for sure to test playability and set as standard for code.

Just a question: how do you find the correct PID parameters? Trial and error or some different procedure?

Finally, could be a good improvement that instead of use "steady" color to indicate gear (or dump) you can set the number of "blink" of the led of the remote (1 "blink" for 1st, 2 "blinks" for 2nd gear, ... and a steady value for dump, for example).

Thank you

[repkovsky]

Thanks!

Regarding PID - I wanted to tune it looking at the motor's step response, but I could not find the reliable method to transmit angle readings to PC, so I ended up doing trial and error experiments :/. For some reason printing current angle in the main loop results in not responding hub and need for total reset, including new bluetooth connection. One method I didn't try was saving the motor angles in a list, and printing it all after the turning is complete, I hope I will be able to check it soon.

When it comes to blinking LED - I had this idea, but I abandoned it for 3 reasons. First - colors are not that hard to remember, and they are visible all the time, so this kind of indicator works quite nice. Second - I tried to make is as simple as possible. Blinking LED is easy when yo do it once and nothing else happens in the background, but if don't want to freeze all other functions for the time of such blinking (which is short, but still noticeable) you would need to do it somehow in the main loop. I'm not sure if the main loop has constant time of one iteration, so proper LED blinking can turn out to be a challenge :). Third - I think I noticed that changing LED color too fast results again in not responding hub... Maybe when remote will be included in stable version, this problem will vanish.

[laurensvalk]

I could not find the reliable method to transmit angle readings to PC

There's built-in logging functionality for this, but it's not documented in great detail. Would you be interested in this?

For some reason printing current angle in the main loop results in not responding hub and need for total reset, including new bluetooth connection

Could you share a code snippet that reproduces this? This this something we'd like to fix.

I think I noticed that changing LED color too fast results again in not responding hub

To confirm, was this changing the remote LED color, or the LED on the hub?

[repkovsky]

Today I tried this printing in main loop, and it was working properly in most cases, as well as remote's LED blinking. Hub hanged only once, but I was not able to find exact cause. One way or another - I was able to collect step responses, so I will try to test some PID parameters and post results here, if anyone is interested.

[laurensvalk]

This is great, thanks so much for sharing!

I would appreciate any comments, corrections as well as sharing it further

We're definitely interested in sharing your example (and those that others have contributed recently). We're aiming to work on the website after we finish some of the upcoming releases :)

[tobobus]

Dear repkovsky,

Iam testing now for 2 days but unfortunate i dont get the 42114 working propely.
I use the beta software the latest update. and the beta app.
Its a little bit difficult to explane because english is not my native lanquese but i give it a try.
i used chrome, the app and edge and it looks like that all the firsts try did work till on/off.
After loading again the steering is ok till trying the gear box then the hub is going in a total hangup and i dont get it out after that.

I reset the hub to the lego orginal and that is working ok.
After that i dont get it working anymore with pybricks.
Sometimes with a another browser i get it working for a moment????

It looks like ther is no total reset or something like that.
I have tested with different hubs and so on no effect.
oh by the way the pybricks with 42124 buggy works ok!!!!

Please would you give me some advice were to look at or what mistake i made.

Regards Tobo.

[repkovsky]

Dear Tobo,

Thank you for the feedback. Unfortunately I don't have another 42114 for testing (maybe I will try to borrow one), so your experience is very important for improving this program. First I would check if the gearbox calibrates properly - after the remote is connected, it should move maximally to the left, to the position visible for example here https://youtu.be/kKGAsx_oDbM?t=844. Then switch to the manual mode (green button) and try to set 2nd gear (right red button). Normally the violet element in gearbox it should move 90 degrees right. What happens in your case?

[Gcopper1984]

Hi @repkovsky,

I think I face a similar problem. Connection with pybricks works OK, when start the script remote connect but then do not happen nothing. The calibration sequence do not start and then everything hang after some seconds, and I need to restart the bricks.

I am not very intro Python code so I am investigating the problem so I do not understand where is the problem yet, can you share (via youtube) the normal way of working of your model?

By the way I am also learning some python with your code :-)

Thank you

[tobobus]

Dear Repkovsky, Thanks so far for your quick response , iam going to test it and let you know asap. Greetings Tobo. Verstuurd vanaf mijn iPad

…

Op 22 okt. 2021 om 22:26 heeft repkovsky ***@***.***> het volgende geschreven:  Dear Tobo, Thank you for the feedback. Unfortunately I don't have another 42114 for testing (maybe I will try to borrow one), so your experience is very important for improving this program. First I would check if the gearbox calibrates properly - after the remote is connected, it should move maximally to the left, to the position visible for example here https://youtu.be/kKGAsx_oDbM?t=844. Then switch to the manual mode (green button) and try to set 2nd gear (right red button). Normally the violet element in gearbox it should move 90 degrees right. What happens in your case? — You are receiving this because you commented. Reply to this email directly, view it on GitHub, or unsubscribe. Triage notifications on the go with GitHub Mobile for iOS or Android.

[tobobus]

@Gcopper1984 ,

youtube from repkovky , see also top of this page

see https://www.youtube.com/watch?v=j4ZxOhXygNY

Tobo

[Gcopper1984]

Hi Tobo,

thank you. I was asking also for a video showing also initialization sequence to understand the calibration process. Mine simple do not calibrate anything and everything is blocked.

It's a really good work also because it is a "real" automatic gearbox, not implementd by lego

[tobobus]

Hi Gcopper,

Indeed it looks like we have the same problem but also i do have no programming experience with pybricks. The strange thing what happened with me is that is was working a few times after i was trying another browser but only till i disconnected and would make a new connetion then it got in a total hangup.
I hope you understand because my second problem is now to explain it in proper english.

[tobobus]

Hi Repkovsky,

I hope the picture is usefull. I have also a picture 42099 sequence in working state but i managed that only once.
Because i dont know if this is ok , let me know if you want them .

Picture in hangup state.

Regards Tobo.

[repkovsky]

Dear Tobo,

thank you for the picture. I have some idea what may be the reason, I will check this out as soon as I will be able to (hopefully tomorrow).

[tobobus]

Thanks Repkovsky, Iam looking forward to it. Tobo. Verstuurd vanaf mijn iPad

…

Op 25 okt. 2021 om 12:46 heeft repkovsky ***@***.***> het volgende geschreven:  Dear Tobo, thank you for the picture. I have some idea what may be the reason, I will check this out as soon as I will be able to (hopefully tomorrow). — You are receiving this because you commented. Reply to this email directly, view it on GitHub, or unsubscribe. Triage notifications on the go with GitHub Mobile for iOS or Android.

[repkovsky]

@tobobus @Gcopper1984 I have pushed new code to the repository, please check if this works for you. Calibration procedure is a bit different than before. Previously it started with rotation of the gearbox motor with limited power to make it precise, but a side effect could be that the power was too small to make any move (which could result in no activity after connecting remote). Now initially the power is set to maximum, and after gearbox motor is moved at all, the precise calibration follows. Additionally, if for some reason gearbox enters hangup state, it will be automatically recalibrated after some time of no activity (1.5 sec), and set to 1st gear.

[Gcopper1984]

Hi @repkovsky,

thank you. I hope I will find some time in the next few days to give you some feedback.

[tobobus]

Dear repkovsky,

I did a quick test and hurray it works. Tomorrow i will test it futher but iam confident that you did a great job so a very very big Thanks you.
More testing tomorrow will let you know.
Regards Tobo and thank you again.

[tobobus]

Dear Repkovky,

After some futher testing, hangup problem solved.
But unfortunate the automatic gear does not work, i have some values from speed but no effect. It stays in first gear.
As off i have no ideer what to change ( if it is software ?) please advice what to do.
Maybe wait of the results from Gcopper 1984 ??

regards Tobo.

[Gcopper1984]

Dear Repkovky,

I finally found some time to make some python tutorial and to test your work.

I think I found the main problem for the model to struggling and stop working, I hope this solve also Tobos trouble.

The main reason that I found is that you set too higher parameters value (speed, acceleration, etc) for movements and simple my model can not do that (I think it is a lazy worker :-) )
To be clear, first thing I noticed is that you use a HI_SPEED value of 1750 [deg/s]. After adding some print(drive.speed()) in while True: loop I discovered that my model simply do not arrive at that speed (maximun value of 1400 - 1500) so my model will never change gear. Only with fresh Alkaline batteries my model can go to 1900 - 1800. Maybe could be that my model has more friction than your and is generally slower, so I set it to 1400. I also lowered the LO_SPEED value.
So I start digging into your code and lowered all the speed and removed all the command related to setting motor parameters. So I lowered the gearbox speed from 2000 to 720 deg/s. With this speed a 90 deg shift is done in 0.125 s with is a good shifting time for a lego assembled plastic toy :-). For all the motors I also used all default PID and standard value.

At this point model start working in a good and predictable way, finally enjoying your really good work. Also your code is really good and I will try to set it as "good practice" for future work.

I really like the remote color code, it is clear a very useful. I try to add better playability adding also hub light to indicate different state of the model to better understand what their are doing.
In details:
. on startup, while hub is waiting to connecting to remote, it blinks white as the remote;
. once connected to the remote it is steady yellow waiting to do calibration
. when calibration is completed hub light turn green and you can play!

I also enjoy the function that check gearbox status. But here I removed the remote red light indicator (it is confusing, I was thinking my model turn in 3rd gear - automatic mode) and instead set red the hub light. After calibration it returns green.

Just a question: steering command are sended only once when a button is pressed or in every while True: loop if the button is pressed? Because while turning my model slow down a lot...

Finally, I also commented the gearbox print function

I hope this help also @tobobus.

I cut and paste the code here and I am trying to open a pull request in github. Tell me if it is ok for you. All my comment in your code are marked with GC84.

# Controlling LEGO Volvo Articulated Hauler (42114) with Bluetooth Remote (88010)
# Version 1.1

# Remote is currently in beta. This program only works with firmware
# installed from <https://beta.pybricks.com>.

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Port, Button, Color
from pybricks.tools import wait

# control buttons swapping
LEFT_STEER_RIGHT_DRIVE = True

# set gearbox to AUTO mode at startup?
INIT_GEARBOX_AUTO = True

# steering settings
STEER_ANGLE = 45 # GC84: lower value
STEER_SPEED = 1000

class Gearbox:
    # if speed is stable above, automatic gearbox will increase gear
    HI_SPEED = 1400 # 1750
    # if speed is stable below, automatic gearbox will decrease gear
    LO_SPEED = 300 # 800
    # time [ms] after gearbox switches to 1st gear when drive remains idle
    GEAR_RESET_TIMEOUT = 2000
    # time [ms] of speed stability measurement before automatic gear change
    STABLE_SPEED_TIME = 800
    # time [ms] for normal gear switch
    GEAR_SWITCH_TIMEOUT = 1500
    # speed measurement smoothing factor
    SMOOTHING = 0.05
    # colors of gearbox state indicator LED for automatic (True) and manual (False)
    # [gear 1, gear 2, gear 3, dumper]
    POS_COLOR = {True: [Color.CYAN, Color.BLUE, Color.MAGENTA, Color.GREEN],
                 False: [Color.ORANGE,  Color(h=15, s=100, v=100),
                         Color(h=5, s=100, v=100), Color.GREEN]}
    def __init__(self, remote):
        # initialize class properties
        self.remote = remote
        self.speed_timer = 0
        self.idle_timer = 0
        self.speed = 0
        # initialize L motor
        self.gearbox = Motor(Port.B)
        # self.gearbox.control.limits(speed=2000, acceleration=4000) # GC84: unsafe value? Left standard value
        self.callibrate()
        # set defaults
        self.prev_gear = 0
        self.set_auto(INIT_GEARBOX_AUTO)

    def callibrate(self):
        # callibrate gearbox motor by finding its physical rotation limit; 
        # first, move left at full power to handle possible jam in gearbox
        self.gearbox.run_until_stalled(360, duty_limit=100) # GC84: lower speed
        # second, correct the position
        self.gearbox.run_angle(360, -90) # GC84: lower speed
        # finally move left with small power to avoid twisting 12-axle and measurement error
        stalled_angle = self.gearbox.run_until_stalled(360, duty_limit=10) # GC84: lower speed
        # round to multiple of 90 degrees and subtract angle of physical block (90deg)
        base_angle = 90*round(stalled_angle/90)-90
        # adjust settings of possible motor positions
        self.pos_angle = [p+base_angle for p in [90, 0, -90, -180]]
        self.pos = 0

    def set_position(self, pos):
        global hub
        # limit positions to range 0,1,2,3
        pos = min(3, max(pos, 0))
        # apply new position if it is different from current one
        if self.pos != pos:
            # set remote control light according to mode and position
            self.remote.light.on(self.POS_COLOR[self.auto][pos])
            # rotate gearbox to angle that corresponds position
            self.gearbox.run_target(720, target_angle=self.pos_angle[pos], wait=False) # # GC84: lower speed from 2000
            change_time = 0
            while not self.gearbox.control.done() and change_time < self.GEAR_SWITCH_TIMEOUT:
                # measure the switching time
                change_time += 1
                wait(1)
            if change_time == self.GEAR_SWITCH_TIMEOUT:
                # GCopper1984 Cool function but it is not clear
                # something went wrong, gearbox position mismatch - set LED to red           
                #self.remote.light.on(Color.RED) # GC84 This create confusion while you are using the model
                hub.light.on(Color.RED) # HC84 Turn red hub light instead
                # stop switching and recallibrate
                self.gearbox.stop()
                self.callibrate()
                pos = 0
                # 1st gear is set
                hub.light.on(Color.GREEN) # GC84 OK, start play!
                self.remote.light.on(self.POS_COLOR[self.auto][pos])
            if pos == 3:
                # remeber gear used before switching to dumper
                self.prev_gear = self.pos
            self.pos = pos
            #print('Position:', pos, self.gearbox.angle(), 
            #      'error:', self.pos_angle[pos]-self.gearbox.angle(),
            #      'auto:', bool(self.auto))

    def dumper(self):
        # return whether gearbox is set to drive dumper
        return self.pos == 3

    def set_auto(self, auto):
        # set AUTO/MANUAL mode and update control light
        self.auto = auto
        self.remote.light.on(self.POS_COLOR[self.auto][self.pos])

    def get_auto_gear(self, speed):
        # returns changed gear if speed is stable below/above LO_SPEED/HI_SPEED threshold
        if self.auto and not self.dumper():
            # basic low-pass filtering (exponential smoothing)
            self.speed += self.SMOOTHING*(abs(speed)-self.speed)
            wait(10)
            if self.LO_SPEED < self.speed < self.HI_SPEED:
                # speed in medium range, reset timer
                self.speed_timer = 0
            else:
                # speed out of medium range, increase time of measurement
                self.speed_timer += 10
                if self.speed_timer > self.STABLE_SPEED_TIME:
                    # speed is stable - reset timer
                    self.speed_timer = 0
                    # depending on speed and current position,
                    # return lower, higher or None gear (no change) 
                    if self.pos > 0 and self.speed < self.LO_SPEED:
                        return self.pos - 1
                    elif self.pos < 2 and self.speed > self.HI_SPEED:
                        return self.pos + 1
        return None

    def idle(self, persists):
        if persists:
            # increase idle time
            wait(1)
            self.idle_timer += 1
            if self.auto and self.idle_timer >= self.GEAR_RESET_TIMEOUT:
                # reset gearbox to lowest gear
                self.idle_timer = 0
                gearbox.set_position(0)
        else:
            self.idle_timer = 0

class Key:
    def __init__(self):
        # variables to store current and previous state of buttons
        self.now_pressed = []
        self.prev_pressed = []
    
    def update(self, remote):
        # copy list of keys pressed during last update
        self.prev_pressed = list(self.now_pressed)
        # update list of pressed keys
        self.now_pressed = remote.buttons.pressed()

    def pressed(self, key):
        # return whether key is now pressed
        return key in self.now_pressed

    def released(self, key):
        # return keys which were released after last update
        return key in self.prev_pressed and key not in self.now_pressed

def direction(positive, negative):
    # return resultant value of two boolean directions
    return int(bool(positive)) - int(bool(negative))

if __name__ == '__main__':
    #print('Start main.')
    connect_flashing_time = [75, 75, 75, 75, 75, 1000]
    hub = TechnicHub()
    hub.light.blink(Color.WHITE, connect_flashing_time) # Flashing led while waiting connection as remote do

    # Connect to the remote.
    remote = Remote()
    #print('Remote connected.')
    hub.light.on(Color.YELLOW) # Wait for calibration

    # initialize driving motor
    drive = Motor(Port.A)

    # initialize steering motor
    steer = Motor(Port.D)
    kp, ki, _, _, _ = steer.control.pid()
    #steer.control.limits(speed=STEER_SPEED, acceleration=3000) # GC84: Use standard value
    #steer.control.pid(kp=kp*2, ki=ki*2, integral_range=40) #steer.control.pid(kp=kp*8, ki=ki*8, integral_range=60) # GC84: use standard value

    # initialize gearbox
    gearbox = Gearbox(remote)

    # initialize remote keys
    key = Key()
    if LEFT_STEER_RIGHT_DRIVE:
        BUTTON_DRIVE_FWD, BUTTON_DRIVE_BACK = Button.RIGHT_PLUS, Button.RIGHT_MINUS
        BUTTON_STEER_LEFT, BUTTON_STEER_RIGHT = Button.LEFT_PLUS, Button.LEFT_MINUS
    else:
        BUTTON_DRIVE_FWD, BUTTON_DRIVE_BACK = Button.LEFT_PLUS, Button.LEFT_MINUS
        BUTTON_STEER_LEFT, BUTTON_STEER_RIGHT = Button.RIGHT_PLUS, Button.RIGHT_MINUS

    #print('Start main loop.')
    hub.light.on(Color.GREEN) # Calibration completed, start the FUN!

    # main loop
    while True:
        # DEBUG: check max speeed
        #print(drive.speed())
        key.update(remote)

        # gearbox control
        if key.released(Button.LEFT):
            # manual - change to lower gear; auto - switch to driving
            new_pos = gearbox.prev_gear if gearbox.auto else gearbox.pos-1
            gearbox.set_position(new_pos)
        elif key.released(Button.RIGHT):
            # manual - change to higher gear/dumper; auto - switch to dumper
            new_pos = 3 if gearbox.auto else gearbox.pos+1
            gearbox.set_position(new_pos)
        elif key.released(Button.CENTER):
            # switch gearbox mode to the other one
            gearbox.set_auto(not gearbox.auto)
        
        # drive control
        drive_direction = direction(key.pressed(BUTTON_DRIVE_FWD),
                                    key.pressed(BUTTON_DRIVE_BACK))
        if drive_direction in [-1,1]:
            auto_gear = gearbox.get_auto_gear(drive.speed())
            if auto_gear is not None:
                # stop motor to allow smooth gear switch
                drive.stop()
                gearbox.set_position(auto_gear)
            # for dumper direction of rotation must be inverted
            invert = 1 if gearbox.dumper() else -1
            drive.dc(invert*drive_direction*100.0)
            # report active drive
            gearbox.idle(False)
        else:
            drive.stop()
            # report idle drive if not set to dumper
            gearbox.idle(not gearbox.dumper())
        
        # steering control
        # GC84: this is always setting target position, button pressed should be fired only one time, return to center should be fired on button button released event
        steer_direction = direction(key.pressed(BUTTON_STEER_RIGHT),
                                    key.pressed(BUTTON_STEER_LEFT))
        steer.run_target(STEER_SPEED, steer_direction*STEER_ANGLE, wait=False)

Thank you

[tobobus]

Dear Gcopper and Repkovky,

I got it also working with the script from Gcopper because also too many friction but also too low voltage from the accus. I change that to alcaline AA and have now 9.5V (fully full ) instaid of 7.2 V with the Ikea Ni-Mh Ladda 2450.

For now I would THANK you both for the efford to get this working. My next step would be to understand more of the code and how to program it.

Best Regards Tobo.

[repkovsky]

Guys, big thanks for testing the code! @Gcopper1984 You are right about changes of HI_SPEED (and LO_SPEED does not hurt to be changed also) - I am using NiZn 1.6V batteries, which probably gives motors much more power than typical batteries. Using hub for showing the status of connection and calibration is also great idea!

Just a question: steering command are sended only once when a button is pressed or in every while True: loop if the button is pressed? Because while turning my model slow down a lot...

It could be done this way, but I don't think the reason for slowing down is sending command run_target to Large Angular Motor, since it is actually sent in every iteration of main loop - if none of the steering buttons are down, then it keeps steering to angle 0. More probable reason is increased friction due to changing direction, or increased load of the battery, as two motors are operating at once. It could be implemented in the way you suggest, but it would require handling both key down, key up, and key pressed (now only key pressed and key up=released are handled).

I have more problems with modification of steering. Two questions:

• Why do you suggest limiting angle to 45 degrees? From my experiments I see that 60 degrees is reachable, and even with this value the turning radius is rather large, so I'd rather avoid making it even bigger.
• What happens in your case with current motor settings? With default settings I see that turning is very shaky and far from smoothness. It works more acceptable while driving, but when hauler does not move, then angular motor visibly struggles with friction. This is fixed with the non-default settings, at least in my case.

[Gcopper1984]

You are welcome!

Glad to ear problem are resolved.

About your question I found that my model slow down a lot while steering and I simple lowered the maximun angle to 45 degress is an acceptable value for me. I also set a lower value for LO_SPEED because sometime model switch gear down while steering...
And, yes, with standard value steering is very shaky like you state, I start some tuning about some parameters. I found your a lot better but a little bit to "hard" (my motor is making noise while stay steady). Maybe a little lower value could be fine.

Could be a very "universal" improvements link the HI_SPEED to the battery voltage (there is a method in hub class I think...) so your model work correctly regardless batteries you use.

Now works very well. I was very interested in a project like this: my biggest complain about new Control+ system is that you MUST use model with your smartphone and this way of use is not funny (very slow, no feedback). Use of the official remote and the liberty to not use a smartphone is great plus.

Thank you one more time.

[tobobus]

Hi Gcopper and Repkovsky,

I have done some testing with voltage-drop while steering and moving.

Idle 8.6 V
Driving 1,2,3, gear 8.3 V
Driving plus steering +/_ 8.2 V

So almost not notacable so please yours comments on that !

regards Tobo.

[repkovsky]

@Gcopper1984 I have finally merged your code into repository (sorry for this long silence on my side). I have modified some parts to make Python code more standard and adjusted PID parameters for steering. The ones which I used initially could be to high, but on the other hand I think defaults are too low, and returning to center takes much too long, so finally I have chosen something in the middle. Once again thanks for cooperation!

[laurensvalk]

Thanks for continuing your work on this!

We've just released a new beta this week. If you haven't updated already, it would be great if you could re-run your code on the latest firmware.

There have been a lot of motor performance updates under the hood, so you may find that some of the tweaks that were previously necessary are no longer needed.

[Gcopper1984]

Hi,

no problem for the long time, everyone has its own life ;-)!

I saw the new release and I hope I will find some spare time to test it in the next few days...

[laurensvalk]

@repkovsky,

If you're interested, it would be awesome if you want to add your project to the pybricks-project repository by making a pull request. Then it will eventually show up on the Pybricks website.

Here's an example of the files and formats to make it work:
https://github.com/pybricks/pybricks-projects/tree/master/sets/technic/42099-off-roader/keyboard-remote

Here's how that project looks on the site:
https://pybricks.com/projects/sets/technic/42099-off-roader/keyboard-remote/

[repkovsky]

Sure! I will just wait for @Gcopper1984 tests to be sure that the current version is acceptable.

[repkovsky]

@laurensvalk

I took me a while to come back to this project, make it compatible with 3.1 version and fix some errors, but finally I managed to create pull request. I am not sure if .md files are written properly, I hope you can review it and fix or give me some hints what should be corrected.