Remote Controlled LIDAR-based GiggleBot - Remote Code

gigglebot_lidar_remote.py

from microbit import sleep, display, button_a, button_b
import ustruct
import radio
import math

radio.on()
rotate_steps = 10
rotate_span = 160 # in degrees
rotate_step = rotate_span / rotate_steps
max_distance = 50 # in centimeters
side_length_leds = 3 # measured in the # of pixels
radar = bytearray(rotate_steps)
xar = bytearray(rotate_steps)
yar = bytearray(rotate_steps)
saved_xar = bytearray(rotate_steps)
saved_yar = bytearray(rotate_steps)
motor_speed = 50

while True:
    status = radio.receive_bytes_into(radar)
    if status is not None:

        # display.clear()
        for c, val in enumerate(radar):
            if radar[c] <= max_distance:
                # calculate 2d coordinates of each distance
                angle = rotate_steps / (rotate_steps - 1) * rotate_step * c
                angle += (180 - rotate_span) / 2.0
                x_c = math.cos(angle * math.pi / 180.0) * radar[c]
                y_c = math.sin(angle * math.pi / 180.0) * radar[c]

                # scale the distances to fit on the 5x5 microbit display
                x_c = x_c * (side_length_leds - 1) / max_distance
                y_c = y_c * (side_length_leds + 1) / max_distance

                # reposition coordinates
                x_c += (side_length_leds - 1)
                y_c = (side_length_leds + 1) - y_c

                # round coordinates exactly where the LEDs are found
                if x_c - math.floor(x_c) < 0.5:
                    x_c = math.floor(x_c)
                else:
                    x_c = math.ceil(x_c)
                if y_c - math.floor(y_c) < 0.5:
                    y_c = math.floor(y_c)
                else:
                    y_c = math.ceil(y_c)

                xar[c] = x_c
                yar[c] = y_c
            else:
                xar[c] = 0
                yar[c] = 0

        display.clear()
        for x, y in zip(xar, yar):
            display.set_pixel(x, y, 9)

        # print(list(zip(xar, yar, radar)))

    stateA = button_a.is_pressed()
    stateB = button_b.is_pressed()

    if stateA and stateB:
        radio.send_bytes(ustruct.pack('bb', motor_speed, motor_speed))
        print('forward')
    if stateA and not stateB:
        radio.send_bytes(ustruct.pack('bb', motor_speed, -motor_speed))
        print('left')
    if not stateA and stateB:
        radio.send_bytes(ustruct.pack('bb', -motor_speed, motor_speed))
        print('right')
    if not stateA and not stateB:
        radio.send_bytes(ustruct.pack('bb', 0, 0))
        print('stop')