Zetros 42129 with lego remote

[Gcopper1984]

Hi,

and here it is script for controlling Mercedes Zetros 42129 with remote controller.

It has a similar logic that my 42124 controller (here: #504) but using the "callback" approach used here #515).

Controlling the model is simple:

. Left + and - to steering
. right + and - to drive at 100% power. If you press also right red button together with right + or - you cut power to 50 %, If you press right red button alone model brake.

. Press the left red button to toogle the lock differential function. If the model has open differential hub and remote led switch to green, if you enable the locked differential both leds turn red.

I am moving my first step in python scripting, so any feedback is welcome.

Here the code:

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

class Remote_Controller:
    # Useful constant
    _PRESSED = 0
    _RELEASED = 1

    # Remote
    remote = None

    # Button status (bitwise operation)
    actual_buttons_status = 0
    previous_buttons_status = actual_buttons_status

    #  Buttons callback
    callback_center_pressed = None
    callback_center_released = None

    callback_left_center_pressed = None
    callback_left_center_released = None

    callback_left_plus_pressed = None
    callback_left_plus_released = None

    callback_left_minus_pressed = None
    callback_left_minus_released = None

    callback_right_center_pressed = None
    callback_right_center_released = None

    callback_right_plus_pressed = None
    callback_right_plus_released = None

    callback_right_minus_pressed = None
    callback_right_minus_released = None

    def __init__(self):
        self.remote = Remote()
        pressed = self.remote.buttons.pressed()
        
        # Check if buttons are pressed
        if Button.CENTER in pressed:
            self.actual_buttons_status += 1 

        if Button.LEFT in pressed:
            self.actual_buttons_status += 2

        if Button.LEFT_PLUS in pressed:
            self.actual_buttons_status += 4

        if Button.LEFT_MINUS in pressed:
            self.actual_buttons_status += 8

        if Button.RIGHT in pressed:
            self.actual_buttons_status += 16

        if Button.RIGHT_PLUS in pressed:
            self.actual_buttons_status += 32

        if Button.RIGHT_MINUS in pressed:
            self.actual_buttons_status += 64

        # Save old values
        self.previous_buttons_status = self.actual_buttons_status

    def register_callback(self, button, action, callback):
        if button == Button.CENTER:
            if action == self._PRESSED:
                self.callback_center_pressed = callback
            elif action == self._RELEASED:
                self.callback_center_released = callback

        if button == Button.LEFT:
            if action == self._PRESSED:
                self.callback_left_center_pressed = callback
            elif action == self._RELEASED:
                self.callback_left_center_released = callback

        if button == Button.LEFT_PLUS:
            if action == self._PRESSED:
                self.callback_left_plus_pressed = callback
            elif action == self._RELEASED:
                self.callback_left_plus_released = callback

        if button == Button.LEFT_MINUS:
            if action == self._PRESSED:
                self.callback_left_minus_pressed = callback
            elif action == self._RELEASED:
                self.callback_left_minus_released = callback

        if button == Button.RIGHT:
            if action == self._PRESSED:
                self.callback_right_center_pressed = callback
            elif action == self._RELEASED:
                self.callback_right_center_released = callback

        if button == Button.RIGHT_PLUS:
            if action == self._PRESSED:
                self.callback_right_plus_pressed = callback
            elif action == self._RELEASED:
                self.callback_right_plus_released = callback

        if button == Button.RIGHT_MINUS:
            if action == self._PRESSED:
                self.callback_right_minus_pressed = callback
            elif action == self._RELEASED:
                self.callback_right_minus_released = callback

    def update(self):
        # Update status
        pressed = self.remote.buttons.pressed()
        self.actual_buttons_status = 0

        # Check if buttons are pressed
        if Button.CENTER in pressed:
            self.actual_buttons_status += 1

        if Button.LEFT in pressed:
            self.actual_buttons_status += 2

        if Button.LEFT_PLUS in pressed:
            self.actual_buttons_status += 4

        if Button.LEFT_MINUS in pressed:
            self.actual_buttons_status += 8

        if Button.RIGHT in pressed:
            self.actual_buttons_status += 16

        if Button.RIGHT_PLUS in pressed:
            self.actual_buttons_status += 32

        if Button.RIGHT_MINUS in pressed:
            self.actual_buttons_status += 64

        # Callback
        if self.actual_buttons_status != self.previous_buttons_status:
            if (self.actual_buttons_status & 1) != (self.previous_buttons_status & 1):
                if self.actual_buttons_status & 1 != 0:
                    if self.callback_center_pressed != None:
                        self.callback_center_pressed()
                else:
                    if self.callback_center_released != None:
                        self.callback_center_released()

            if (self.actual_buttons_status & 2) != (self.previous_buttons_status & 2):
                if self.actual_buttons_status & 2 != 0:
                    if self.callback_left_center_pressed != None:
                        self.callback_left_center_pressed()
                else:
                    if self.callback_left_center_released != None:
                        self.callback_left_center_released()
                        
            if (self.actual_buttons_status & 4) != (self.previous_buttons_status & 4):
                if self.actual_buttons_status & 4 != 0:
                    if self.callback_left_plus_pressed != None:
                        self.callback_left_plus_pressed()
                else:
                    if self.callback_left_plus_released != None:
                        self.callback_left_plus_released()
                        
            if (self.actual_buttons_status & 8) != (self.previous_buttons_status & 8):
                if self.actual_buttons_status & 8 != 0:
                    if self.callback_left_minus_pressed != None:
                        self.callback_left_minus_pressed()
                else:
                    if self.callback_left_minus_released != None:
                        self.callback_left_minus_released()
                        
            if (self.actual_buttons_status & 16) != (self.previous_buttons_status & 16):
                if self.actual_buttons_status & 16 != 0:
                    if self.callback_right_center_pressed != None:
                        self.callback_right_center_pressed()
                else:
                    if self.callback_right_center_released != None:
                        self.callback_right_center_released()
                        
            if (self.actual_buttons_status & 32) != (self.previous_buttons_status & 32):
                if self.actual_buttons_status & 32 != 0:
                    if self.callback_right_plus_pressed != None:
                        self.callback_right_plus_pressed()
                else:
                    if self.callback_right_plus_released != None:
                        self.callback_right_plus_released()
                        
            if (self.actual_buttons_status & 64) != (self.previous_buttons_status & 64):
                if self.actual_buttons_status & 64 != 0:
                    if self.callback_right_minus_pressed != None:
                        self.callback_right_minus_pressed()
                else:
                    if self.callback_right_minus_released != None:
                        self.callback_right_minus_released()

        # Save old values
        self.previous_buttons_status = self.actual_buttons_status

def init_steering():
    global steering_motor
    global angle_right
    global angle_left
    global angle_zero

    angle_right = steering_motor.run_until_stalled(-200, duty_limit=75, then=Stop.HOLD)
    angle_left = steering_motor.run_until_stalled(200, duty_limit=75, then=Stop.HOLD)
    angle_zero = (angle_left + angle_right) / 2
    steering_motor.run_target(200, angle_zero, then=Stop.HOLD , wait=False)

def steer_left():
    global steering_motor
    global angle_left
    steering_motor.run_target(2000, angle_left + 5, then=Stop.HOLD, wait=False)

def steer_right():
    global steering_motor
    global angle_right
    steering_motor.run_target(2000, angle_right - 5, then=Stop.HOLD, wait=False)

def return_to_center():
    global steering_motor
    global angle_zero
    steering_motor.run_target(2000, angle_zero, then=Stop.HOLD, wait=False)

def press_power_plus():
    global direction
    global power
    global brake

    direction = 1
    power = 1

    #print("Press motor power plus ", direction, power, brake)
    set_drive_motor()

def press_power_minus():
    global direction
    global power
    global brake

    direction = -1
    power = 1

    #print("Press motor power minus ", direction, power, brake)
    set_drive_motor()
    
def release_power():
    global direction
    global power
    global brake

    direction = 0
    power = 0

    #print("Release power ", direction, power, brake)
    set_drive_motor()

def press_power_brake():
    global direction
    global power
    global brake

    brake = 1

    #print("Press power brake ", direction, power, brake)
    set_drive_motor()

def release_power_brake():
    global direction
    global power
    global brake

    brake = 0

    #print("Release power brake ", direction, power, brake)
    set_drive_motor()

def init_differential():
    global lock_motor
    global diff_is_locked

    lock_differential_motor.dc(50)
    wait(750)
    lock_differential_motor.stop()
    diff_is_locked = False

def lock_differential():
    global lock_motor
    global diff_is_locked
    global remote_controller
    global hub 
    
    if diff_is_locked == False:
        lock_differential_motor.dc(-50)
        wait(750)
        lock_differential_motor.stop()
        diff_is_locked = True
        remote_controller.remote.light.on(Color.RED)
        hub.light.on(Color.RED)
    else:
        lock_differential_motor.dc(50)
        wait(750)
        lock_differential_motor.stop()
        diff_is_locked = False
        remote_controller.remote.light.on(Color.GREEN)
        hub.light.on(Color.GREEN)

def set_drive_motor():
    global drive_motor_1
    global drive_motor_2
    global direction
    global power
    global brake

    if power == 0 and brake == 0:
        drive_motor_1.stop()
        drive_motor_2.stop()
    elif power == 0 and brake == 1:
        drive_motor_1.brake()
        drive_motor_2.brake()
    elif power > 0 and brake == 0:
        drive_motor_1.dc(direction * 100)
        drive_motor_2.dc(direction * 100)
    elif power > 0 and brake == 1:
        drive_motor_1.dc(direction * 50)
        drive_motor_2.dc(direction * 50)

# *** Main ***
print("Start main program")

# Init devices
hub = TechnicHub()
connect_flashing_time = [75, 75, 75, 75, 75, 1000]
hub.light.blink(Color.WHITE, connect_flashing_time)

steering_motor = Motor(Port.D)
[kp, kd, ki, maxi, ratei] = steering_motor.control.pid()
steering_motor.control.pid(7 * kp, 2 * kd, ki, 5 * maxi, 5 * ratei)
angle_right = 0
angle_left = 0
angle_zero = 0
init_steering()

drive_motor_1 = Motor(Port.A)
drive_motor_2 = Motor(Port.B)
direction = 0
power = 0
brake = 0

lock_differential_motor = DCMotor(Port.C)
diff_is_locked = False
init_differential()

print("Press center button on remote to connect...")
remote_controller = Remote_Controller()
remote_controller.remote.light.on(Color.GREEN)
hub.light.on(Color.GREEN)

remote_controller.register_callback(Button.LEFT, remote_controller._PRESSED, lock_differential)
remote_controller.register_callback(Button.LEFT_PLUS, remote_controller._PRESSED, steer_left)
remote_controller.register_callback(Button.LEFT_PLUS, remote_controller._RELEASED, return_to_center)
remote_controller.register_callback(Button.LEFT_MINUS, remote_controller._PRESSED, steer_right)
remote_controller.register_callback(Button.LEFT_MINUS, remote_controller._RELEASED, return_to_center)

remote_controller.register_callback(Button.RIGHT, remote_controller._PRESSED, press_power_brake)
remote_controller.register_callback(Button.RIGHT, remote_controller._RELEASED, release_power_brake)
remote_controller.register_callback(Button.RIGHT_PLUS, remote_controller._PRESSED, press_power_plus)
remote_controller.register_callback(Button.RIGHT_PLUS, remote_controller._RELEASED, release_power)
remote_controller.register_callback(Button.RIGHT_MINUS, remote_controller._PRESSED, press_power_minus)
remote_controller.register_callback(Button.RIGHT_MINUS, remote_controller._RELEASED, release_power)

print("Press any button and see effect")

while True:
    remote_controller.update()