AutonPark.v5python

{"mode":"Text","hardwareTarget":"brain","textContent":"#region VEXcode Generated Robot Configuration\nfrom vex import *\nimport urandom\nimport math\n\n# Brain should be defined by default\nbrain=Brain()\n\n# Robot configuration code\ncontroller_1 = Controller(PRIMARY)\nleft_motor_a = Motor(Ports.PORT8, GearSetting.RATIO_6_1, False)\nleft_motor_b = Motor(Ports.PORT18, GearSetting.RATIO_6_1, False)\nleft_motor_c = Motor(Ports.PORT6, GearSetting.RATIO_6_1, False)\nleft_drive_smart = MotorGroup(left_motor_a, left_motor_b, left_motor_c)\nright_motor_a = Motor(Ports.PORT3, GearSetting.RATIO_6_1, True)\nright_motor_b = Motor(Ports.PORT13, GearSetting.RATIO_6_1, True)\nright_motor_c = Motor(Ports.PORT15, GearSetting.RATIO_6_1, True)\nright_drive_smart = MotorGroup(right_motor_a, right_motor_b, right_motor_c)\ndrivetrain_inertial = Inertial(Ports.PORT5)\ndrivetrain = SmartDrive(left_drive_smart, right_drive_smart, drivetrain_inertial, 219.44, 320, 40, MM, 0.5)\nFrontLandM = Motor(Ports.PORT11, GearSetting.RATIO_18_1, True)\nTopMotors = Motor(Ports.PORT1, GearSetting.RATIO_18_1, True)\nBackMiddle = Motor(Ports.PORT10, GearSetting.RATIO_18_1, False)\nBackLower = Motor(Ports.PORT20, GearSetting.RATIO_18_1, False)\n# AI Vision Color Descriptions\nai_vision_1__RedBlock = Colordesc(1, 124, 40, 61, 12, 0.25)\nai_vision_1__BlueBlock = Colordesc(2, 67, 163, 228, 13, 0.3)\n# AI Vision Code Descriptions\nai_vision_1 = AiVision(Ports.PORT16, ai_vision_1__RedBlock, ai_vision_1__BlueBlock)\nBumperFront = DigitalOut(brain.three_wire_port.a)\nHook = DigitalOut(brain.three_wire_port.b)\n\n\n# wait for rotation sensor to fully initialize\nwait(30, MSEC)\n\n\n# Make random actually random\ndef initializeRandomSeed():\n    wait(100, MSEC)\n    random = brain.battery.voltage(MV) + brain.battery.current(CurrentUnits.AMP) * 100 + brain.timer.system_high_res()\n    urandom.seed(int(random))\n      \n# Set random seed \ninitializeRandomSeed()\n\nvexcode_initial_drivetrain_calibration_completed = False\ndef calibrate_drivetrain():\n    # Calibrate the Drivetrain Inertial\n    global vexcode_initial_drivetrain_calibration_completed\n    sleep(200, MSEC)\n    brain.screen.print(\"Calibrating\")\n    brain.screen.next_row()\n    brain.screen.print(\"Inertial\")\n    drivetrain_inertial.calibrate()\n    while drivetrain_inertial.is_calibrating():\n        sleep(25, MSEC)\n    vexcode_initial_drivetrain_calibration_completed = True\n    brain.screen.clear_screen()\n    brain.screen.set_cursor(1, 1)\n\n\n# Calibrate the Drivetrain\ncalibrate_drivetrain()\n\n\ndef play_vexcode_sound(sound_name):\n    # Helper to make playing sounds from the V5 in VEXcode easier and\n    # keeps the code cleaner by making it clear what is happening.\n    print(\"VEXPlaySound:\" + sound_name)\n    wait(5, MSEC)\n\n# add a small delay to make sure we don't print in the middle of the REPL header\nwait(200, MSEC)\n# clear the console to make sure we don't have the REPL in the console\nprint(\"\\033[2J\")\n\n\n\n# define variables used for controlling motors based on controller inputs\ndrivetrain_l_needs_to_be_stopped_controller_1 = False\ndrivetrain_r_needs_to_be_stopped_controller_1 = False\n\n# define a task that will handle monitoring inputs from controller_1\ndef rc_auto_loop_function_controller_1():\n    global drivetrain_l_needs_to_be_stopped_controller_1, drivetrain_r_needs_to_be_stopped_controller_1, remote_control_code_enabled\n    # process the controller input every 20 milliseconds\n    # update the motors based on the input values\n    while True:\n        if remote_control_code_enabled:\n            # stop the motors if the brain is calibrating\n            if drivetrain_inertial.is_calibrating():\n                left_drive_smart.stop()\n                right_drive_smart.stop()\n                while drivetrain_inertial.is_calibrating():\n                    sleep(25, MSEC)\n            \n            # calculate the drivetrain motor velocities from the controller joystick axies\n            # left = axis3 + axis1\n            # right = axis3 - axis1\n            drivetrain_left_side_speed = controller_1.axis3.position() + controller_1.axis1.position()\n            drivetrain_right_side_speed = controller_1.axis3.position() - controller_1.axis1.position()\n            \n            # check if the value is inside of the deadband range\n            if drivetrain_left_side_speed < 5 and drivetrain_left_side_speed > -5:\n                # check if the left motor has already been stopped\n                if drivetrain_l_needs_to_be_stopped_controller_1:\n                    # stop the left drive motor\n                    left_drive_smart.stop()\n                    # tell the code that the left motor has been stopped\n                    drivetrain_l_needs_to_be_stopped_controller_1 = False\n            else:\n                # reset the toggle so that the deadband code knows to stop the left motor next\n                # time the input is in the deadband range\n                drivetrain_l_needs_to_be_stopped_controller_1 = True\n            # check if the value is inside of the deadband range\n            if drivetrain_right_side_speed < 5 and drivetrain_right_side_speed > -5:\n                # check if the right motor has already been stopped\n                if drivetrain_r_needs_to_be_stopped_controller_1:\n                    # stop the right drive motor\n                    right_drive_smart.stop()\n                    # tell the code that the right motor has been stopped\n                    drivetrain_r_needs_to_be_stopped_controller_1 = False\n            else:\n                # reset the toggle so that the deadband code knows to stop the right motor next\n                # time the input is in the deadband range\n                drivetrain_r_needs_to_be_stopped_controller_1 = True\n            \n            # only tell the left drive motor to spin if the values are not in the deadband range\n            if drivetrain_l_needs_to_be_stopped_controller_1:\n                left_drive_smart.set_velocity(drivetrain_left_side_speed, PERCENT)\n                left_drive_smart.spin(FORWARD)\n            # only tell the right drive motor to spin if the values are not in the deadband range\n            if drivetrain_r_needs_to_be_stopped_controller_1:\n                right_drive_smart.set_velocity(drivetrain_right_side_speed, PERCENT)\n                right_drive_smart.spin(FORWARD)\n        # wait before repeating the process\n        wait(20, MSEC)\n\n# define variable for remote controller enable/disable\nremote_control_code_enabled = True\n\nrc_auto_loop_thread_controller_1 = Thread(rc_auto_loop_function_controller_1)\n\n#endregion VEXcode Generated Robot Configuration\n\n# Begin project code\nai_vision_1_objects = []\nscreen_precision = 0\nconsole_precision = 0\n\nbuttonXActive = False\nbuttonBActive = False\nbuttonUpActive = False\nbuttonLRActive = False\nbuttonDownActive = False\nmiddleOutputActive = False\nintakeFunctActive = False\n\ndrivetrain.set_drive_velocity(100, PERCENT)\n\ndef buttonBFunct():\n    BackMiddle.stop()\n    TopMotors.stop()\n    FrontLandM.stop()\n    BackLower.stop()\n    buttonXActive = False\n    buttonUpActive = False\n    buttonLRActive = False\n    buttonDownActive = False\n    middleOutputActive = False\n    intakeFunctActive = False\n    global buttonBActive\n    if not buttonBActive:\n        FrontLandM.spin(FORWARD)\n        BackLower.spin(REVERSE)\n        buttonBActive = True\n    else:\n        FrontLandM.stop()\n        TopMotors.stop()\n        BackLower.stop()\n        buttonBActive = False\n\ndef buttonXFunct():\n    BackMiddle.stop()\n    TopMotors.stop()\n    FrontLandM.stop()\n    BackLower.stop()\n    buttonBActive = False\n    buttonUpActive = False\n    buttonLRActive = False\n    buttonDownActive = False\n    middleOutputActive = False\n    intakeFunctActive = False\n    global buttonXActive\n    if not buttonXActive:\n        FrontLandM.spin(FORWARD)\n        BackLower.spin(FORWARD)\n        BackMiddle.spin(REVERSE)\n        buttonXActive = True\n    else:\n        FrontLandM.stop()\n        BackLower.stop()\n        BackMiddle.stop()\n        buttonXActive = False\n\ndef buttonUpFunct():\n    BackMiddle.stop()\n    TopMotors.stop()\n    FrontLandM.stop()\n    BackLower.stop()\n    buttonBActive = False\n    buttonXActive = False\n    buttonLRActive = False\n    buttonDownActive = False\n    middleOutputActive = False\n    intakeFunctActive = False\n    global buttonUpActive\n    if not buttonUpActive:\n        FrontLandM.spin(FORWARD)\n        BackLower.spin(REVERSE)\n        TopMotors.spin(FORWARD)\n        BackMiddle.spin(FORWARD)\n        buttonUpActive = True\n    else:\n        FrontLandM.stop()\n        BackLower.stop()\n        TopMotors.stop()\n        BackMiddle.stop()\n        buttonUpActive = False\n\ndef buttonLRFunct():\n    BackMiddle.stop()\n    TopMotors.stop()\n    FrontLandM.stop()\n    BackLower.stop()\n    buttonBActive = False\n    buttonXActive = False\n    buttonUpActive = False\n    buttonDownActive = False\n    middleOutputActive = False\n    intakeFunctActive = False\n    global buttonLRActive\n    if not buttonLRActive:\n        FrontLandM.spin(FORWARD)\n        BackLower.spin(REVERSE)\n        BackMiddle.spin(FORWARD)\n        TopMotors.spin(REVERSE)\n        buttonLRActive = True\n    else:\n        FrontLandM.stop()\n        BackLower.stop()\n        BackMiddle.stop()\n        TopMotors.stop()\n        buttonLRActive = False\n\ndef buttonDownFunct():\n    BackMiddle.stop()\n    TopMotors.stop()\n    FrontLandM.stop()\n    BackLower.stop()\n    buttonBActive = False\n    buttonXActive = False\n    buttonUpActive = False\n    buttonLRActive = False\n    middleOutputActive = False\n    intakeFunctActive = False\n    global buttonDownActive\n    if not buttonDownActive:\n        FrontLandM.spin(REVERSE)\n        BackMiddle.spin(FORWARD)\n        BackLower.spin(REVERSE)\n        buttonDownActive = True\n    else:\n        FrontLandM.stop()\n        BackMiddle.stop()\n        BackLower.stop()\n        buttonDownActive = False\n\ndef stopall():\n    FrontLandM.stop()\n    TopMotors.stop()\n    BackMiddle.stop()\n    BackLower.stop()\n    global buttonBActive, buttonXActive, buttonUpActive, buttonLRActive, buttonDownActive, middleOutputActive, intakeFunctActive\n    buttonBActive = False\n    buttonXActive = False\n    buttonUpActive = False\n    buttonLRActive = False\n    buttonDownActive = False\n    middleOutputActive = False\n    intakeFunctActive = False\n\nbumperFrontValue = False\n\ndef bumperFunct():\n    global bumperFrontValue\n    if bumperFrontValue:\n        BumperFront.set(False)\n        bumperFrontValue = False\n    else:\n        BumperFront.set(True)\n        bumperFrontValue = True\n\nhookValue = False\n\ndef hookFunct():\n    global hookValue\n    if hookValue:\n        Hook.set(False)\n        hookValue = False\n    else:\n        Hook.set(True)\n        hookValue = True\n\nupOutputActive = False\n\ndef upOutputStart():\n    global upOutputActive\n    if not upOutputActive:\n        upOutputThread = Thread(upOutput)\n\ndef upOutput():\n    global ai_vision_1_objects, screen_precision, console_precision, upOutputActive\n    upOutputActive = True\n    while controller_1.buttonUp.pressing():\n        TopMotors.spin(FORWARD)\n        BackMiddle.spin(FORWARD)\n        FrontLandM.spin(FORWARD)\n        ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__RedBlock)\n        if ai_vision_1_objects[0].width >= 270:\n            if ai_vision_1_objects[0].centerX >= 150 and ai_vision_1_objects[0].centerX <= 170:\n                brain.screen.clear_row(1)\n                brain.screen.set_cursor(brain.screen.row(), 1)\n                brain.screen.set_cursor(1, 1)\n                brain.screen.print(\"Detected Red\")\n                BackLower.spin(FORWARD)\n                wait(1, SECONDS)\n        else:\n            BackLower.spin(REVERSE)\n            brain.screen.clear_row(1)\n            brain.screen.set_cursor(brain.screen.row(), 1)\n        ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__BlueBlock)\n        if ai_vision_1_objects[0].width >= 150:\n            if ai_vision_1_objects[0].centerX >= 100 and ai_vision_1_objects[0].centerX <= 200:\n                brain.screen.clear_row(2)\n                brain.screen.set_cursor(brain.screen.row(), 1)\n                brain.screen.set_cursor(2, 1)\n                brain.screen.print(\"Detected Blue\")\n                BackLower.spin(FORWARD)\n        else:\n            BackLower.spin(REVERSE)\n            brain.screen.clear_row(2)                \n            brain.screen.set_cursor(brain.screen.row(), 1)\n        wait(5, MSEC)\n    FrontLandM.stop()\n    TopMotors.stop()\n    BackMiddle.stop()\n    BackLower.stop()\n    FrontLandM.stop()\n    wait(5, MSEC)\n    upOutputActive = False    \n\nmiddleOutputActive = False\n\ndef middleOutputStart():\n    global middleOutputActive\n    if not middleOutputActive:\n        middleOutputThread = Thread(middleOutput)\n\ndef middleOutput():\n    global ai_vision_1_objects, screen_precision, console_precision, middleOutputActive\n    middleOutputActive = True\n    while controller_1.buttonLeft.pressing() or controller_1.buttonRight.pressing():\n        FrontLandM.spin(FORWARD)\n        TopMotors.spin(REVERSE)\n        BackMiddle.spin(FORWARD)\n        FrontLandM.spin(FORWARD)\n        ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__RedBlock)\n        if ai_vision_1_objects[0].width >= 270:\n            if ai_vision_1_objects[0].centerX >= 150 or ai_vision_1_objects[0].centerX <= 170:\n                brain.screen.clear_row(1)\n                brain.screen.set_cursor(brain.screen.row(), 1)\n                brain.screen.set_cursor(1, 1)\n                brain.screen.print(\"Detected Red\")\n                BackLower.spin(FORWARD)\n                wait(1, SECONDS)\n        else:\n            BackLower.spin(REVERSE)\n            brain.screen.clear_row(1)\n            brain.screen.set_cursor(brain.screen.row(), 1)\n        ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__BlueBlock)\n        if ai_vision_1_objects[0].width >= 150:\n            if ai_vision_1_objects[0].centerX >= 100 or ai_vision_1_objects[0].centerX <= 200:\n                brain.screen.clear_row(2)\n                brain.screen.set_cursor(brain.screen.row(), 1)\n                brain.screen.set_cursor(2, 1)\n                brain.screen.print(\"Detected Blue\")\n                BackLower.spin(FORWARD)\n        else:\n            BackLower.spin(REVERSE)\n            brain.screen.clear_row(2)                \n            brain.screen.set_cursor(brain.screen.row(), 1)\n        wait(5, MSEC)\n    FrontLandM.stop()\n    TopMotors.stop()\n    BackMiddle.stop()\n    BackLower.stop()\n    FrontLandM.stop()\n    wait(5, MSEC)\n    middleOutputActive = False\n\nintakeFunctActive = False\n\ndef intakeFunct():\n    global ai_vision_1_objects, screen_precision, console_precision, intakeFunctActive\n\n    intakeFunctActive = True\n\n    while intakeFunctActive:\n\n        FrontLandM.spin(FORWARD)\n        BackLower.spin(FORWARD)\n\n        # ---------- RED BLOCK ----------\n        ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__RedBlock)\n        found_red = False\n\n        for obj in ai_vision_1_objects:\n            if obj.width >= 70 and 140 <= obj.centerX <= 175:\n                brain.screen.clear_row(1)\n                brain.screen.set_cursor(1, 1)\n                brain.screen.print(\"Detected Red\")\n\n                BackLower.spin(REVERSE)\n                BackMiddle.stop()\n\n                wait(0.5, SECONDS)\n                found_red = True\n                break\n\n        if not found_red:\n            brain.screen.clear_row(1)\n            brain.screen.set_cursor(1, 1)\n\n        # ---------- BLUE BLOCK ----------\n        ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__BlueBlock)\n        found_blue = False\n\n        for obj in ai_vision_1_objects:\n            if obj.width >= 70 and 140 <= obj.centerX <= 170:\n                brain.screen.clear_row(2)\n                brain.screen.set_cursor(2, 1)\n                brain.screen.print(\"Detected Blue\")\n\n                BackLower.spin(FORWARD)\n                BackMiddle.spin(REVERSE)\n\n                wait(0.5, SECONDS)\n                found_blue = True\n                break\n\n        if not found_blue:\n            brain.screen.clear_row(2)\n            brain.screen.set_cursor(1, 1)\n\n        wait(5, MSEC)\n\n    # ---------- STOP MOTORS ----------\n    FrontLandM.stop()\n    TopMotors.stop()\n    BackMiddle.stop()\n    BackLower.stop()\n    wait(5, MSEC)\n\n\ndef buttonAFunct():\n    global intakeFunctActive\n    if not intakeFunctActive:\n        intakeFunctThread = Thread(intakeFunct)\n    else:\n        intakeFunctActive = False\n        brain.screen.clear_row(1)\n        brain.screen.clear_row(2)\n\ncontroller_1.buttonA.pressed(buttonAFunct)\ncontroller_1.buttonR2.pressed(bumperFunct)\ncontroller_1.buttonL2.pressed(stopall)\ncontroller_1.buttonL1.pressed(stopall)\ncontroller_1.buttonR1.pressed(bumperFunct)\ncontroller_1.buttonX.pressed(buttonXFunct)\ncontroller_1.buttonB.pressed(buttonBFunct)\ncontroller_1.buttonUp.pressed(upOutputStart)\ncontroller_1.buttonLeft.pressed(middleOutputStart)\ncontroller_1.buttonRight.pressed(middleOutputStart)\ncontroller_1.buttonDown.pressed(buttonDownFunct)\n\n# def checkBlue():\n#     global myVariable, ai_vision_1_objects, screen_precision, console_precision\n#     brain.screen.set_font(FontType.MONO40)\n#     brain.screen.clear_row(1)\n#     brain.screen.set_cursor(brain.screen.row(), 1)\n#     brain.screen.set_cursor(1, 1)\n#     ai_vision_1_objects = ai_vision_21.take_snapshot(ai_vision_21__BlueBlock)\n#     if ai_vision_1_objects and len(ai_vision_1_objects) > 0:\n#         brain.screen.print(\"Blue Object Found\")\n#     else:\n#         brain.screen.print(\"No Blue Object\")\n\n# def checkRed():\n#     global myVariable, ai_vision_1_objects, screen_precision, console_precision\n#     brain.screen.set_font(FontType.MONO40)\n#     brain.screen.clear_row(3)\n#     brain.screen.set_cursor(brain.screen.row(), 1)\n#     brain.screen.set_cursor(3, 1)\n#     ai_vision_1_objects = ai_vision_21.take_snapshot(ai_vision_21__RedBlock)\n#     if ai_vision_1_objects and len(ai_vision_1_objects) > 0:\n#         brain.screen.print(\"Red Object Found\")\n#     else:\n#         brain.screen.print(\"No Red Object\")\n\ndef pre_autonomous():\n    # actions to do when the program starts\n    brain.screen.clear_screen()\n    brain.screen.print(\"pre auton code\")\n    wait(1, SECONDS)\n\ndef autonomous():\n    # global ai_vision_1_objects, screen_precision, console_precision\n\n    # brain.screen.clear_screen()\n    # brain.screen.print(\"autonomous code\")\n\n    # # ---------- INITIAL MOVEMENT ----------\n    # BackMiddle.spin(REVERSE)\n    # FrontLandM.spin(FORWARD)\n    # BackLower.spin(FORWARD)\n\n    # drivetrain.drive_for(FORWARD, 900, MM)\n    # brain.screen.print(drivetrain_inertial.heading(DEGREES))\n\n    # drivetrain.turn_to_heading(130, DEGREES)\n    # drivetrain.drive_for(FORWARD, 880, MM)\n\n    # drivetrain.turn_to_heading(170, DEGREES)\n    # bumperFunct()\n    # drivetrain.drive_for(FORWARD, 200, MM)\n\n    # wait(3, SECONDS)\n\n    # drivetrain.drive_for(REVERSE, 250, MM)\n    # bumperFunct()\n    # drivetrain.turn_to_heading(350, DEGREES)\n\n    # BackLower.stop()\n    # BackMiddle.stop()\n    # FrontLandM.stop()\n\n    # drivetrain.drive_for(FORWARD, 475, MM)\n\n    # # ---------- VISION SECTION (5 SECONDS) ----------\n    # brain.timer.reset()\n\n    # while brain.timer.time(SECONDS) < 5:\n\n    #     # Drive motors\n\n    #     TopMotors.spin(FORWARD)\n    #     BackMiddle.spin(FORWARD)\n    #     FrontLandM.spin(FORWARD)\n\n    #     # ---------- RED BLOCK DETECTION ----------\n    #     ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__RedBlock)\n    #     found_red = False\n\n    #     for obj in ai_vision_1_objects:\n    #         if obj.width >= 270 and 150 <= obj.centerX <= 170:\n    #             brain.screen.clear_row(1)\n    #             brain.screen.set_cursor(1, 1)\n    #             brain.screen.print(\"Detected Red\")\n\n    #             BackLower.spin(FORWARD)\n    #             wait(1, SECONDS)\n\n    #             found_red = True\n    #             break\n\n    #     if not found_red:\n    #         BackLower.spin(REVERSE)\n    #         brain.screen.clear_row(1)\n\n    #     # ---------- BLUE BLOCK DETECTION ----------\n    #     ai_vision_1_objects = ai_vision_1.take_snapshot(ai_vision_1__BlueBlock)\n    #     found_blue = False\n\n    #     for obj in ai_vision_1_objects:\n    #         if obj.width >= 150 and 100 <= obj.centerX <= 200:\n    #             brain.screen.clear_row(2)\n    #             brain.screen.set_cursor(2, 1)\n    #             brain.screen.print(\"Detected Blue\")\n\n    #             BackLower.spin(FORWARD)\n\n    #             found_blue = True\n    #             break\n\n    #     if not found_blue:\n    #         BackLower.spin(REVERSE)\n    #         brain.screen.clear_row(2)\n\n    #     # Prevent CPU overload\n    #     wait(20, MSEC)\n\n    # # ---------- STOP MOTORS AFTER AUTON ----------\n    # TopMotors.stop()\n    # BackMiddle.stop()\n    # FrontLandM.stop()\n    # BackLower.stop()\n    # BackMiddle.spin(REVERSE)\n    # FrontLandM.spin(FORWARD)\n    # BackLower.spin(FORWARD)\n    # drivetrain.drive_for(REVERSE, 200, MM)\n    # drivetrain.turn_to_heading(270, DEGREES)\n    # drivetrain.drive_for(FORWARD, 1800, MM)\n    # drivetrain.turn_to_heading(220, DEGREES)\n    # drivetrain.drive_for(FORWARD, 950, MM)\n\n    # drivetrain.turn_to_heading(180, DEGREES)\n    # bumperFunct()\n    # drivetrain.drive_for(FORWARD, 200, MM)\n\n    # wait(3, SECONDS)\n\n    # drivetrain.drive_for(REVERSE, 250, MM)\n    # bumperFunct()\n    # drivetrain.turn_to_heading(350, DEGREES)\n    # drivetrain.drive_for(FORWARD, 475, MM)\n    # drivetrain.turn_to_heading(135, DEGREES)\n    # drivetrain.drive_for(FORWARD, 2000, MM)\n    buttonXFunct()\n    drivetrain.set_drive_velocity(75, PERCENT)\n    controller_1.screen.clear_screen()\n    brain.screen.set_cursor(1, 1)\n    controller_1.screen.print(\"FORWARD, 650, MM\")\n    drivetrain.drive_for(FORWARD, 650, MM)\n    wait(1, SECONDS)\n    controller_1.screen.set_cursor(2, 1)\n    controller_1.screen.print(\"REVERSE, 175, MM\")\n    drivetrain.drive_for(REVERSE, 175, MM)\n\ndef user_control():\n    # global myVariable, ai_vision_1_objects, screen_precision, console_precision\n    # while True:\n        # checkBlue()\n        # checkRed()\n        # wait(0.1, SECONDS)\n        # wait(20, MSEC)\n    brain.screen.clear_screen()\n    # place driver control in this while loop\n    while True:\n        wait(20, MSEC)\n\ndef FrontLMForward():\n    if FrontLandM.is_spinning():\n        FrontLandM.stop()\n    else:\n        FrontLandM.spin(FORWARD)\n\ndef FrontLMReverse():\n    if FrontLandM.is_spinning():\n        FrontLandM.stop()\n    else:\n        FrontLandM.spin(REVERSE)\n\ndef TopMotorForward():\n    if TopMotors.is_spinning():\n        TopMotors.stop()\n    else:\n        TopMotors.spin(FORWARD)\n\ndef TopMotorReverse():\n    if TopMotors.is_spinning():\n        TopMotors.stop()\n    else:\n        TopMotors.spin(REVERSE)\n\ndef BackMiddleForward():\n    if BackMiddle.is_spinning():\n        BackMiddle.stop()\n    else:\n        BackMiddle.spin(FORWARD)\n\ndef BackMiddleReverse():\n    if BackMiddle.is_spinning():\n        BackMiddle.stop()\n    else:\n        BackMiddle.spin(REVERSE)\n\ndef BackLowerForward():\n    if BackLower.is_spinning():\n        BackLower.stop()\n    else:\n        BackLower.spin(FORWARD)\n\ndef BackLowerReverse():\n    if BackLower.is_spinning():\n        BackLower.stop()\n    else:\n        BackLower.spin(REVERSE)\n\nTopMotors.set_velocity(100, PERCENT)\nFrontLandM.set_velocity(100, PERCENT)\nBackMiddle.set_velocity(100, PERCENT)\nBackLower.set_velocity(100, PERCENT)\n\n# controller_1.buttonA.pressed(FrontLMForward)\n# controller_1.buttonB.pressed(FrontLMReverse)\n# controller_1.buttonX.pressed(TopMotorForward)\n# controller_1.buttonY.pressed(TopMotorReverse)\n# controller_1.buttonR1.pressed(BackMiddleForward)\n# controller_1.buttonR2.pressed(BackMiddleReverse)\n# controller_1.buttonL1.pressed(BackLowerForward)\n# controller_1.buttonL2.pressed(BackLowerReverse)\n\n# create competition instance\ncomp = Competition(user_control, autonomous)\npre_autonomous()","textLanguage":"python","robotConfig":[{"port":[],"name":"controller_1","customName":false,"deviceType":"Controller","deviceClass":"controller","setting":{"left":"","leftDir":"false","right":"","rightDir":"false","upDown":"","upDownDir":"false","xB":"","xBDir":"false","drive":"split","id":"primary"},"triportSourcePort":22},{"port":[8,18,3,13,5],"name":"drivetrain","customName":false,"deviceType":"Drivetrain","deviceClass":"smartdrive","setting":{"type":"4-motor","wheelSize":"wheel275in","gear":"ratio6_1","gearRatio":"1:2","direction":"fwd","gyroType":"inertial","width":"295","unit":"mm","wheelbase":"40","wheelbaseUnit":"mm","xOffset":"0","xOffsetUnit":"mm","yOffset":"0","yOffsetUnit":"mm","thetaOffset":"180"},"triportSourcePort":null},{"port":[11],"name":"FrontLandM","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[1],"name":"TopMotors","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[10],"name":"BackMiddle","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1","id":"partner"},"triportSourcePort":22},{"port":[20],"name":"BackLower","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1","id":"partner"},"triportSourcePort":22},{"port":[16],"name":"ai_vision_1","customName":true,"deviceType":"AIVision","deviceClass":"aivision","setting":{"config":"{\"colors\":[{\"id\":1,\"name\":\"RedBlock\",\"nameValid\":\"Valid\",\"lastValidName\":\"RedBlock\",\"red\":123.57600115517906,\"green\":40.114266461301504,\"blue\":60.57020119368502,\"hueRange\":12,\"saturationRange\":0.25},{\"id\":2,\"name\":\"BlueBlock\",\"nameValid\":\"Valid\",\"lastValidName\":\"BlueBlock\",\"red\":67.32487980769231,\"green\":163.48858173076923,\"blue\":228.07079326923076,\"hueRange\":13,\"saturationRange\":0.3}],\"codes\":[],\"tags\":false,\"AIObjects\":false,\"AIObjectModel\":[\"Ball(Blue)\",\"Ball(Green)\",\"Ball(Red)\",\"Ring(Blue)\",\"Ring(Green)\",\"Ring(Red)\",\"Cube(Blue)\",\"Cube(Green)\",\"Cube(Red)\"],\"AIModelMetadata\":{\"id\":17,\"version\":1,\"name\":\"2025-2026 Push Back\"},\"aiModelDropDownValue\":0}","isConfigured":"false"},"triportSourcePort":22},{"port":[1],"name":"BumperFront","customName":true,"deviceType":"DigitalOut","deviceClass":"digital_out","setting":{"id":"partner"},"triportSourcePort":22},{"port":[2],"name":"Hook","customName":true,"deviceType":"DigitalOut","deviceClass":"digital_out","setting":{"id":"partner"},"triportSourcePort":22}],"slot":4,"platform":"V5","sdkVersion":"20240802.15.00.00","appVersion":"4.64.0","minVersion":"4.60.0","fileFormat":"2.0.0","targetBrainGen":"First","v5Sounds":[{"name":"game over","url":"static/sounds/mixkit-arcade-retro-game-over-213.wav"}],"v5SoundsEnabled":false,"aiVisionSettings":{"colors":[],"codes":[],"tags":true,"AIObjects":true,"AIObjectModel":[],"aiModelDropDownValue":null},"target":"Physical"}