robot malfunction

[OPSLeoGong]

from pybricks.hubs import PrimeHub
from pybricks.iodevices import UARTDevice
from pybricks.parameters import Port, Direction, Stop, Button, Icon
from pybricks.tools import wait, StopWatch, multitask, run_task
from pybricks.pupdevices import Motor, ForceSensor
from pybricks.robotics import DriveBase
from micropython import mem_info

hub = PrimeHub()

cam = UARTDevice(Port.D)
cam.set_baudrate(115200)
lm = Motor(Port.A, Direction.COUNTERCLOCKWISE)
rm = Motor(Port.E, Direction.CLOCKWISE)
kicker = Motor(Port.C, reset_angle=False)
catcher = Motor(Port.F, reset_angle=False)
touch = ForceSensor(Port.B)
drivebase = DriveBase(lm, rm, 62.4, 140)
drivebase.settings(900)
drivebase.use_gyro(True)
COLOR_ORANGE = 0
COLOR_PURPLE = 1
MAX_BALLS = 10
OFF_SCREEN = 300
grabbed = False
ox1 = oy1 = ox2 = oy2 = ox3 = oy3 = ox4 = oy4 = ox5 = oy5 = ox6 = oy6 = ox7 = oy7 = ox8 = oy8 = OFF_SCREEN
orange_count = 0

px1 = py1 = px2 = py2 = px3 = py3 = px4 = py4 = px5 = py5 = px6 = py6 = px7 = py7 = px8 = py8 = OFF_SCREEN
purple_count = 0

buf = bytearray()

def u16(b0, b1): return b0 | (b1 << 8)

def purge(n):
    global buf
    if n >= len(buf):
        buf = bytearray()
    else:
        new_buf = bytearray()
        new_buf.extend(buf[n:])
        buf = new_buf

def checksum(start, length):
    total = 0
    end = start + length
    for i in range(start, min(end, len(buf))):
        total = (total + buf[i]) & 0xFF
    return total

def parse_one():
    global buf, orange_count, purple_count
    global ox1, oy1, ox2, oy2, ox3, oy3, ox4, oy4, ox5, oy5, ox6, oy6, ox7, oy7, ox8, oy8
    global px1, py1, px2, py2, px3, py3, px4, py4, px5, py5, px6, py6, px7, py7, px8, py8

    if len(buf) < 6:
        return False
    if buf[0] != 0xAA or buf[1] != 0x55:
        purge(1)
        return False

    n = buf[2]
    color_id = buf[3]
    if n == 0 or n > MAX_BALLS or color_id not in (COLOR_ORANGE, COLOR_PURPLE):
        purge(2)
        return False

    need = 6 + 4 * n
    if len(buf) < need:
        return False

    if checksum(2, 3 + 4 * n) != buf[need - 1]:
        purge(1)
        return False

    blobs = []
    p = 5
    for _ in range(n):
        x = u16(buf[p], buf[p + 1]); p += 2
        y = u16(buf[p], buf[p + 1]); p += 2
        blobs.append((x, y))

    purge(need)

    if color_id == COLOR_ORANGE:
        ox1 = oy1 = ox2 = oy2 = ox3 = oy3 = ox4 = oy4 = ox5 = oy5 = ox6 = oy6 = ox7 = oy7 = ox8 = oy8 = OFF_SCREEN
        orange_count = len(blobs)
        if orange_count >= 1:  ox1, oy1 = blobs[0]
        if orange_count >= 2:  ox2, oy2 = blobs[1]
        if orange_count >= 3:  ox3, oy3 = blobs[2]
        if orange_count >= 4:  ox4, oy4 = blobs[3]
        if orange_count >= 5:  ox5, oy5 = blobs[4]
        if orange_count >= 6:  ox6, oy6 = blobs[5]
        if orange_count >= 7:  ox7, oy7 = blobs[6]
        if orange_count >= 8:  ox8, oy8 = blobs[7]
        if orange_count == 0:
            print("ORANGE: none")
        elif orange_count == 1:
            print(f"ORANGE: {ox1},{oy1}  (1 ball)")
        else:
            parts = [f"{x},{y}" for x, y in blobs]
            print(" ".join(parts), f"({orange_count} balls)")

    else: 
        px1 = py1 = px2 = py2 = px3 = py3 = px4 = py4 = px5 = py5 = px6 = py6 = px7 = py7 = px8 = py8 = OFF_SCREEN
        purple_count = len(blobs)

        if purple_count >= 1:  px1, py1 = blobs[0]
        if purple_count >= 2:  px2, py2 = blobs[1]
        if purple_count >= 3:  px3, py3 = blobs[2]
        if purple_count >= 4:  px4, py4 = blobs[3]
        if purple_count >= 5:  px5, py5 = blobs[4]
        if purple_count >= 6:  px6, py6 = blobs[5]
        if purple_count >= 7:  px7, py7 = blobs[6]
        if purple_count >= 8:  px8, py8 = blobs[7]

        if purple_count == 0:
            print("PURPLE: none")
        elif purple_count == 1:
            print(f"PURPLE: {px1},{py1}  (1 ball)")
        else:
            parts = [f"{x},{y}" for x, y in blobs]
            print(" ".join(parts), f"({purple_count} balls)")

    return True
    
    
def one_orange():
    global grabbed
    global orange_count
    grabbed = False
    if orange_count == 1 and purple_count == 0:
        print('Orange is 1')
        error = ox1 - 120
        GAIN = 3.8 if abs(error) > 70 else \
            3.2 if abs(error) > 40 else \
            2.8 if abs(error) > 20 else 2.2

        BASE_SPEED = 1050 if oy1 > 200 else \
                    950 if oy1 > 160 else \
                    850 if oy1 > 120 else 800

        if oy1 <= 200:
            lm.run(BASE_SPEED + error * GAIN)
            rm.run(BASE_SPEED - error * GAIN)
            grabbed = False
        elif not grabbed:
            print("GRABBING SINGLE BALL!")
            catcher.run_target(200000, 90, then=Stop.HOLD, wait=False)
            drivebase.settings(900)
            drivebase.straight(180, then=Stop.COAST, wait=False)
            wait(100)
            catcher.run_target(-90000, -20, then=Stop.HOLD, wait=True)
            drivebase.straight(-100)
            grabbed = True
            print("GRABBED!")
            target = 25
            while True:
                heading = hub.imu.heading()
                error = (target - heading + 180) % 360 - 180
                if abs(error) < 2:
                    lm.stop()
                    rm.stop()
                    break
                speed = 400 if abs(error) > 30 else 250
                lm.run(speed if error > 0 else -speed)
                rm.run(speed if error < 0 else -speed)
            catcher.run_target(2000, 90, then=Stop.HOLD, wait=False)
            wait(330)
            kicker.dc(-1000000)
            wait(1000)
            kicker.run_target(20000, 19, then=Stop.HOLD, wait=False)
            drivebase.stop()
        
        grabbed = False     
        orange_count = 0
def two_orangemore():
    global grabbed
    grabbed = False
    if orange_count >= 2 and purple_count == 0:
        print('Orange is 2+')
        if purple_count >= 1:
            if orange_count == 2:
                if oy1 > oy2:
                    error = ox1 - 120
                    GAIN = 2 if (ox1 > 180 or ox1 < 60) else 1.2
                    BASE_SPEED = 875 if oy1 <= 80 else 675
                    lm.run(BASE_SPEED + error * GAIN)
                    rm.run(BASE_SPEED - error * GAIN)

        else:
            best_y = -1
            best_x = 120
            for i in range(orange_count):
                y = globals()[f"oy{i+1}"]
                x = globals()[f"ox{i+1}"]
                if y > best_y:
                    best_y = y
                    best_x = x

            error = best_x - 120

            GAIN = 4.5 if abs(error) > 80 else \
                    3.8 if abs(error) > 50 else \
                    3.2 if abs(error) > 30 else \
                    2.6 if abs(error) > 15 else 2.0

            BASE_SPEED = 1100 if best_y > 200 else \
                            1050 if best_y > 170 else \
                            1000 if best_y > 140 else \
                            950 if best_y > 110 else 900

            lm.run(BASE_SPEED + error * GAIN)
            rm.run(BASE_SPEED - error * GAIN)

def only_purple():
    grabbed = False
    if orange_count == 0 and purple_count == 1:
        print("PURPLE BALLS ONLY")
        lm.run(200)
        rm.run(-200)
        grabbed = False
def no_balls():
    grabbed = False
    if orange_count == 0 and purple_count == 0:
        print("NO BALLS")
        lm.run(200)
        rm.run(-200)
        grabbed = False


hub.imu.reset_heading(0)
kicker.run_target(500, 19, then=Stop.HOLD, wait=False)
catcher.run_target(20000, 90, then=Stop.HOLD, wait=False)
while True:
    one_orange()
    two_orangemore()
    no_balls()
    only_purple()
    print(purple_count)
    data = cam.read()
    buf.extend(data)

    while len(buf) >= 6:
        if not parse_one():
            break

#kicker.run_target(500, 19, then=Stop.HOLD, wait=True)
# catcher.run_target(500, 45, then=Stop.HOLD, wait=True)
# kicker.dc(-100)

my robot spins when orange_count is 0 and when there is a ball it goes towards it and then the robot shoots but then it keeps repeating going forward and going backward and repeats this part:

print("GRABBING SINGLE BALL!")
            catcher.run_target(200000, 90, then=Stop.HOLD, wait=False)
            drivebase.settings(900)
            drivebase.straight(180, then=Stop.COAST, wait=False)
            wait(100)
            catcher.run_target(-90000, -20, then=Stop.HOLD, wait=True)
            drivebase.straight(-100)
            grabbed = True
            print("GRABBED!")
            target = 25
            while True:
                heading = hub.imu.heading()
                error = (target - heading + 180) % 360 - 180
                if abs(error) < 2:
                    lm.stop()
                    rm.stop()
                    break
                speed = 400 if abs(error) > 30 else 250
                lm.run(speed if error > 0 else -speed)
                rm.run(speed if error < 0 else -speed)
            catcher.run_target(2000, 90, then=Stop.HOLD, wait=False)
            wait(330)
            kicker.dc(-1000000)
            wait(1000)
            kicker.run_target(20000, 19, then=Stop.HOLD, wait=False)
            drivebase.stop()

please someone help

[BertLindeman]

Hello,

Without trying to understand your program:
If a drivebase robot spins it might be that the orientation of the robot is incorrect, or the left and right motor have been plugged in the wrong port.

Maybe first check that in a small program to be sure that this is not the problem?

Bert