Driver Skills.v5python

{"mode":"Text","textContent":"#region VEXcode Generated Robot Configuration\nfrom vex import *\nimport urandom\n\n# Brain should be defined by default\nbrain=Brain()\n\n# Robot configuration code\ncontroller_1 = Controller(PRIMARY)\nCatapult_motor_a = Motor(Ports.PORT6, GearSetting.RATIO_36_1, False)\nCatapult_motor_b = Motor(Ports.PORT10, GearSetting.RATIO_36_1, True)\nCatapult = MotorGroup(Catapult_motor_a, Catapult_motor_b)\nleft_motor_a = Motor(Ports.PORT2, GearSetting.RATIO_18_1, False)\nleft_motor_b = Motor(Ports.PORT4, GearSetting.RATIO_18_1, False)\nleft_drive_smart = MotorGroup(left_motor_a, left_motor_b)\nright_motor_a = Motor(Ports.PORT1, GearSetting.RATIO_18_1, True)\nright_motor_b = Motor(Ports.PORT3, GearSetting.RATIO_18_1, True)\nright_drive_smart = MotorGroup(right_motor_a, right_motor_b)\ndrivetrain_inertial = Inertial(Ports.PORT9)\ndrivetrain = SmartDrive(left_drive_smart, right_drive_smart, drivetrain_inertial, 319.19, 320, 40, MM, 1)\nWing_A = DigitalOut(brain.three_wire_port.a)\nWing_B = DigitalOut(brain.three_wire_port.b)\n\n\n# wait for rotation sensor to fully initialize\nwait(30, MSEC)\n\ndef calibrate_drivetrain():\n    # Calibrate the Drivetrain Inertial\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    brain.screen.clear_screen()\n    brain.screen.set_cursor(1, 1)\n\n\n\n# define variables used for controlling motors based on controller inputs\ndrivetrain_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_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 + axis4\n            # right = axis3 - axis4\n            drivetrain_left_side_speed = controller_1.axis3.position() + controller_1.axis4.position()\n            drivetrain_right_side_speed = controller_1.axis3.position() - controller_1.axis4.position()\n            \n            # check if the values are inside of the deadband range\n            if abs(drivetrain_left_side_speed) < 5 and abs(drivetrain_right_side_speed) < 5:\n                # check if the motors have already been stopped\n                if drivetrain_needs_to_be_stopped_controller_1:\n                    # stop the drive motors\n                    left_drive_smart.stop()\n                    right_drive_smart.stop()\n                    # tell the code that the motors have been stopped\n                    drivetrain_needs_to_be_stopped_controller_1 = False\n            else:\n                # reset the toggle so that the deadband code knows to stop the motors next\n                # time the input is in the deadband range\n                drivetrain_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_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_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#endregion VEXcode Generated Robot Configuration\n\n# ------------------------------------------\n# \n# \tProject:      Main Drive Script\n#\tAuthor:       Daniel Dew, Daniel DaSliva\n#\tCreated:      2023/4\n#\tDescription:  Code for St Christopher's robotics team - contains both autonomous and driver control scripting\n# \n# ------------------------------------------\n\n# Library imports\nfrom vex import *\nimport math\n\n\"\"\"\n------ The Autonomous Period: ------\nGoals:\n- Move Objective triball into goal\n- Navigate Back to match load position\n\nExcecution:\n\n-- Method 1: --\nFull autonomous navigation - using the inertial sensor to estimate position,\nthen navigate along path.\n\nStructure:\n\nSensor -> Navigation system (position est.) -> Pure Pursuit (Error to path)\n-> PID(s) (Move to path)\n\nPros: Allows for hardware errors (slight drifts and/or turns) to be negated\nCons: Complex and subject to sensor noise\n\nWe decided against implementing this for our first competition, as their were to many\nparameters to tune and not enough time to test them.\nHowver, we do plan on implementing this on further competitions - as it shows our robot's\nability to adapt to different paths.\n\n-- Method 2: --\n'Hard Coded Pathfinding' - using path to formulate step-by-step commands.\n\nStructure:\nPath -> Commands\ne.g\nA path of\n[0, 0]\n[0, 1000]\n[-1000, 1000]\n\nGoes to Commands:\n[0, 1000]\n[270, 1000]\n\nIn the Commands array, the first element of each command is the heading required to align\nthe robot to the next path point then the seccond element is the required ammount of forward movement,\nin MM.\n\nPros:\nSimple - easy to implement with VEX API\n\nCons:\nMargin for Error - if drift or hardware errors occur, the robot cannot correc for\nthem, so any errors made stack up and can accumulate into large errors.\nNo smoothed path following - whithout pure pursuit (a program to intercept the path),\nthe robot follows the points, and not an interpolated path between them. This can cause\nthe robot to make jerky, sharp turns.\n\n\n\"\"\"\n\n\n\"\"\"\nFor the autonomous period, there are 2 starting positions, therefore the path for\nrequired for each is different, one requires a left turn, while the other requires a right turn.\nSo, we use two paths and select one before the start.\n\nThe arrays have 3 elements, the first 2 are x and y position values\nand the 3rd one is wether the robot should reverse to the position (False) or drive normally\nto it (True). The last one is wether the robot should use increased speed, in order to push the ball under the goal.\n\"\"\"\n\n\nAutoPath1 = [\n    [0, 950, False, False],\n    [-340, 950, False, False],\n    [200, 940, True, False],\n    [-340, 940, True, True, True],\n    [200, 950, False, False],\n    [-600, 0, False, False, False]\n]\n\nAutoPath2 = [\n    [0, 940, False, False],\n    [300, 950, False, False],\n    [-200, 940, True, False],\n    [290, 940, True, True],\n    [-100, 940, True, False],\n    [600, 0, False, False]\n]\nCanPress = True\n#Function to display path choices - used before comp start\ndef DisplayChoices():\n    CanPress = True\n \n    brain.screen.set_fill_color(Color.WHITE)\n    brain.screen.set_pen_color(Color.BLACK)\n    brain.screen.set_cursor(240, 120)\n    brain.screen.print(\"SELECT AUTONOMOUS PATH\")\n\n    wait(1, SECONDS)\n    \n    # Box 1\n    brain.screen.clear_screen()\n    brain.screen.set_fill_color(Color.BLUE)\n    brain.screen.draw_rectangle(0, 0, 240, 240)\n    brain.screen.set_cursor(10, 0)\n    brain.screen.print(\"PATH 1\")\n\n    # Box 2\n    brain.screen.set_fill_color(Color.RED)\n    brain.screen.draw_rectangle(240, 0, 240, 240)\n    brain.screen.set_cursor(20, 0)\n    brain.screen.print(\"PATH 2\")\nAutoPathChosen = AutoPath1\n# Function to register which path has been selected\ndef ChoosePath():\n    global CanPress\n    if not CanPress:\n        return\n    x = brain.screen.x_position()\n    y = brain.screen.y_position()\n    #Check which box has been pressed and asign Chosen Path to corresponding path\n    if x > 0 and x < 240:\n        AutoPathChosen = AutoPath1\n    elif x >= 240:\n        AutoPathChosen = AutoPath2\n    else:\n        return\n    CanPress = False\n    brain.screen.clear_screen()\n    brain.screen.set_cursor(1,1)\n    brain.screen.print(\"Chosen Path\")\n\n#Function to calculate heading of vector a\ndef HeadingFromVector(a):\n    if a[0] < 0 and a[1] >= 0:\n        return 450 - math.degrees(math.atan2(a[1], a[0]))\n    else:\n        return 90 - math.degrees(math.atan2(a[1], a[0]))\n# Function to calculate magnitude of vector a\ndef Magnitude(a):\n    b = (a[0]**2)+(a[1]**2)\n    return math.sqrt(b)\n# We had problems with the default 'turn_to_heading() function, so this is a replacement that halts\n# the program while we turn to the correct heading\ndef WaitUntilHeading(Heading, error):\n    while True:\n        wait(5, MSEC)\n        if drivetrain_inertial.heading(DEGREES) <= Heading+error and drivetrain_inertial.heading(DEGREES) >= Heading-error:\n            break\n\n\"\"\"\nThis Function Calculates the shortest rotation between the current heading 'T' and the\ntarget heading 'C'\n\"\"\"\ndef ShortestRotation(T, C):\n    diff = abs(T-C)\n\n    if diff <= 180:\n        return LEFT if C > T else RIGHT\n    else:\n        return LEFT if C < T else RIGHT\n# test function to monitor heading during auto period\ndef HeadingDisp():\n    while True:\n        brain.screen.clear_screen()\n        brain.screen.set_cursor(1,1)\n        brain.screen.print(drivetrain_inertial.heading(DEGREES))\n        wait(0.02, SECONDS)\n# -- On holding of the button X, we want the catapult to continously move until X is released\ndef OnXPress():\n    Catapult.set_velocity(40, PERCENT)\n    Catapult.spin(REVERSE)\ndef OnXRelease():\n    Catapult.stop()\ndef OnBPress():\n    Catapult.set_velocity(20, PERCENT)\n    Catapult.spin(FORWARD)\ndef OnBRelease():\n    Catapult.stop()\n\nA_Wing = False\nB_Wing = False\ndef OnUpArrowPress():\n    global A_Wing\n    global B_Wing\n    A_Wing = not A_Wing\n    B_Wing = not B_Wing\n    Wing_A.set(A_Wing)\n    Wing_B.set(B_Wing)\ndef OnLeftBumperPress():\n    global A_Wing\n    A_Wing = not A_Wing\n    Wing_A.set(A_Wing)\ndef OnRightBumperPress():\n    global B_Wing\n    B_Wing = not B_Wing\n    Wing_B.set(B_Wing)\n\n# -- \n# Main autonomous function\ndef autonomous():\n    drivetrain_inertial.calibrate()\n    drivetrain_inertial.set_heading(180,DEGREES)\n    wait(2,SECONDS)\n    # Set turn velocity to low value for accuracy\n    drivetrain.set_turn_velocity(10, PERCENT)\n\n    Commands = []\n    First = True\n    # Loops through path formulating commands\n    for x in range(len(AutoPathChosen)):\n        # 'V2Pos' is the vector from one path point to the next\n        # Calculated by V = [Xn - Xn-1, Yn - Yn-1]\n        # However, if we are on the first element, Xn-1 and Yn-1 do not exist, so we can asign them to 0\n        # Making the Vector simply [Xn - 0, Yn - 0] -> [Xn, Yn]\n        WingExpands = []\n        if len(AutoPathChosen[x]) == 5:\n            WingExpands.append(x)\n            brain.screen.print(\"l\")\n        if First:\n            First = False\n            V2Pos = [AutoPathChosen[x][0], AutoPathChosen[x][1]]\n        else:\n            V2Pos = [AutoPathChosen[x][0]-AutoPathChosen[x-1][0], AutoPathChosen[x][1]-AutoPathChosen[x-1][1]]\n\n        # Calculate heading of vector, to be used in commands\n        Heading2Pos = HeadingFromVector(V2Pos)\n\n        # Magnitude to determine how much the robot needs to move forward or backward\n        DriveDist = Magnitude(V2Pos)\n\n        # If the robot needs to reverse to position, 180 Deg must be added to the heading\n        if AutoPathChosen[x][2] != True:\n            Heading2Pos = (180+Heading2Pos) % 360\n        else:\n            DriveDist = -DriveDist\n        \n        # Add heading and forward movement to command list\n        Commands.append([Heading2Pos, DriveDist, AutoPathChosen[x][3]])\n        \n    # Loop through commands and excecute them\n    for x in Commands:\n        # Uses 'ShortestRotation()' to decide wether to turn left or right\n\n        if (x == WingExpands[0]):\n            OnUpArrowPress()\n        # -- Replaces drivetrain.turn_to_heading()\n        drivetrain.turn(ShortestRotation(x[0], drivetrain_inertial.heading(DEGREES)))\n        WaitUntilHeading(x[0], 2)\n        drivetrain.stop()\n        # --\n        if x[2] == True:\n            drivetrain.set_drive_velocity(60, PERCENT)\n        else:\n            drivetrain.set_drive_velocity(40, PERCENT)\n        drivetrain.drive_for(REVERSE, x[1], MM, wait=True)\n\n#\n\n\n\n\n# Most of the driver control code is handled withn VEX's built in drivetrain configurator\ndef driver_control():\n    drivetrain.stop()\n    drivetrain.set_stopping(BRAKE)\n    drivetrain.set_drive_velocity(50, PERCENT)\n    drivetrain.set_turn_velocity(35, PERCENT)\n    # Loop\n    while True:\n        wait(15, MSEC)\n        brain.screen.set_pen_color(Color.RED)\n        brain.screen.clear_screen()\n        if Catapult_motor_a.temperature(PERCENT) >= 60:\n\n            brain.screen.set_cursor(0, 0)\n            brain.screen.print(\"TEMP OVER 60%, 1\")\n        if Catapult_motor_b.temperature(PERCENT) >= 60:\n            brain.screen.set_cursor(10, 0)\n            brain.screen.print(\"TEMP OVER 60%, 2\")\n\n\n# Pre-Start calibration\nCatapult.set_velocity(100, PERCENT)\n\nCatapult.set_stopping(HOLD)\n\ncontroller_1.buttonX.pressed(OnXPress)\ncontroller_1.buttonX.released(OnXRelease)\ncontroller_1.buttonUp.pressed(OnUpArrowPress)\ncontroller_1.buttonL2.pressed(OnLeftBumperPress)\ncontroller_1.buttonR2.pressed(OnRightBumperPress)\ncontroller_1.buttonB.pressed(OnBPress)\ncontroller_1.buttonB.released(OnBRelease)\n# COMMENT OUT THIS AT START \ndriver_control()","textLanguage":"python","rconfig":[{"port":[],"name":"controller_1","customName":false,"deviceType":"Controller","deviceClass":"controller","setting":{"left":"","leftDir":"false","right":"","rightDir":"true","upDown":"","upDownDir":"false","xB":"","xBDir":"false","drive":"arcadel","id":"primary"},"triportSourcePort":22},{"port":[6,10],"name":"Catapult","customName":true,"deviceType":"MotorGroup","deviceClass":"motor_group","setting":{"fwd":"forward","rev":"reverse","gear":"ratio36_1","motor_a_reversed":"false","motor_b_reversed":"true"},"triportSourcePort":22},{"port":[2,4,1,3,9],"name":"drivetrain","customName":false,"deviceType":"Drivetrain","deviceClass":"smartdrive","setting":{"type":"4-motor","wheelSize":"wheel4in","gear":"ratio18_1","gearRatio":"1:1","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":[1],"name":"Wing_A","customName":true,"deviceType":"DigitalOut","deviceClass":"digital_out","setting":{"id":"partner"},"triportSourcePort":22},{"port":[2],"name":"Wing_B","customName":true,"deviceType":"DigitalOut","deviceClass":"digital_out","setting":{"id":"partner"},"triportSourcePort":22}],"slot":1,"platform":"V5","sdkVersion":"20220726.10.00.00","appVersion":"2.4.5","minVersion":"2.4.0","fileFormat":"1.2.0","icon":"","targetBrainGen":"First","target":"Physical"}