IQ (2nd generation) default drive code

A while back I wrote some code for the IQ 2nd generation that simulates the default drive code inside the brain. It’s not possible to completely simulate the code as some control and motor reversal is done through the control screen for that code, however, those same changes (arcade, tank drive etc.) can be made by modifying the examples. I have the same example in both C++ and Python, you will notice how similar they are.

They are not fully debugged, but I though I would post the two VEXcode projects now as the latest release supports Python.

It does have a UI showing basic data for 8 motors.
hard to see here, but left drive motors in blue, right drive motors in red, generic claw/arm motors in green.

This follows the format of the V5 version I posted earlier this year.

iq2_py_def

python version
#-----------------------------------------------------------------------------*/
#                                                                             */
#     Module:       IQ2_default.py                                            */
#     Author:       james                                                     */
#     Created:      Mon Sept 26 2021                                          */
#     Description:  Default Clawbot code - Python                             */
#                                                                             */
#-----------------------------------------------------------------------------*/

#region VEXcode Generated Robot Configuration
from vex import *
import urandom

# Brain should be defined by default
brain=Brain()

# Robot configuration code
brain_inertial = Inertial()



#endregion VEXcode Generated Robot Configuration

# Create motors
motor_1 = Motor(Ports.PORT1, False)
motor_4 = Motor(Ports.PORT4, False)
motor_5 = Motor(Ports.PORT5, False)
motor_6 = Motor(Ports.PORT6, False)
motor_7 = Motor(Ports.PORT7, False)
motor_10 = Motor(Ports.PORT10, False)
motor_11 = Motor(Ports.PORT11, False)
motor_12 = Motor(Ports.PORT12, False)

# Limit switches for motor 4 and 10
limit_switch_4f = Bumper(Ports.PORT3)
limit_switch_4r = Bumper(Ports.PORT2)
limit_switch_10f = Bumper(Ports.PORT9)
limit_switch_10r = Bumper(Ports.PORT8)

# The controller
controller_1 = Controller()

# Assign generic motor to more useful names here
# We use references
left_drive_1 = motor_1
right_drive_1 = motor_6
left_drive_2 = motor_7
right_drive_2 = motor_12

# Arm and claw motors will have brake mode set to hold
# Claw motor will have max torque limited
claw_motor = motor_4
arm_motor = motor_10

class DriveType:
    LEFT = 0
    DUAL = 1
    SPLIT = 2
    RIGHT = 3
    value = 0

    def __init__(self, value):
        self.value = value
    def __str__(self):
        return self.value
    def __repr__(self):
        return self.value
    def __eq__(self, value):
        return self.value == value

# pick LEFT, DUAL, SPLIT or RIGHT
drive_mode = DriveType(DriveType.LEFT)

# Max motor speed (percent) for motors controlled by buttons
MAX_SPEED = 50

#-----------------------------------------------------------------------------*/
#   @brief  Drive task                                                        */
#-----------------------------------------------------------------------------*/
#
# All motors are controlled from this function which is run as a separate thread
#
def drive_task():
    drive_left = 0
    drive_right = 0

    # right generally needs reversal
    right_drive_1.set_reversed(True);
    right_drive_2.set_reversed(True);

    # setup the claw motor
    claw_motor.set_max_torque(25, PERCENT)
    claw_motor.set_stopping(HOLD)

    # setup the arm motor
    arm_motor.set_stopping(HOLD)

    # loop forever
    while True:
        # buttons
        # Three values, max, 0 and -max.
        #
        drive_m_4   = (controller_1.buttonRUp.pressing() - controller_1.buttonRDown.pressing()) * MAX_SPEED
        drive_m_5   = (controller_1.buttonFUp.pressing() - controller_1.buttonFDown.pressing()) * MAX_SPEED
        drive_m_10  = (controller_1.buttonLUp.pressing() - controller_1.buttonLDown.pressing()) * MAX_SPEED
        drive_m_11  = (controller_1.buttonEUp.pressing() - controller_1.buttonEDown.pressing()) * MAX_SPEED

        # use limit switches on motors 3 and 8
        if (limit_switch_4f.pressing() and (drive_m_4 > 0)) or (limit_switch_4r.pressing() and (drive_m_4 < 0)):
            drive_m_4 = 0
        if (limit_switch_10f.pressing() and (drive_m_10 > 0)) or (limit_switch_10r.pressing() and (drive_m_10 < 0)):
            drive_m_10 = 0

        # Various choices for arcade or tank drive
        if drive_mode == DriveType.LEFT:
            drive_left = controller_1.axisA.position() + controller_1.axisB.position()
            drive_right = controller_1.axisA.position() - controller_1.axisB.position()
        elif drive_mode == DriveType.DUAL:
            drive_left = controller_1.axisA.position()
            drive_right = controller_1.axisD.position()
        elif drive_mode == DriveType.SPLIT:
            drive_left = controller_1.axisD.position() + controller_1.axisB.position()
            drive_right = controller_1.axisD.position() - controller_1.axisB.position()
        elif drive_mode == DriveType.RIGHT:
            drive_left = controller_1.axisD.position() + controller_1.axisC.position()
            drive_right = controller_1.axisD.position() - controller_1.axisC.position()

        # threshold the variable channels so the drive does not
        # move if the joystick axis does not return exactly to 0
        deadband = 15
        if abs(drive_left) < deadband:
            drive_left = 0
        if abs(drive_right) < deadband:
            drive_right = 0

        # Now send all drive values to motors

        # The drivetrain
        left_drive_1.spin(FORWARD, drive_left, PERCENT)
        left_drive_2.spin(FORWARD, drive_left, PERCENT)
        right_drive_1.spin(FORWARD, drive_right, PERCENT)
        right_drive_2.spin(FORWARD, drive_right, PERCENT)

        # and all the auxilary motors
        motor_4.spin(FORWARD, drive_m_4, PERCENT)
        motor_5.spin(FORWARD, drive_m_5, PERCENT)
        motor_10.spin(FORWARD, drive_m_10, PERCENT)
        motor_11.spin(FORWARD, drive_m_11, PERCENT)

        # No need to run too fast
        sleep(10)

# define some more colors
grey = Color(0x202020)
dgrey = Color(0x2F4F4F)
lblue = Color(0x303060)
lred = Color(0x603030)

#------------------------------------------------------------------------------*/
#   @brief      Display data for one motor                                     */
#------------------------------------------------------------------------------*/
def displayMotorData(m, index):
    ypos = 0
    xpos = 0

    if index < 4:
      xpos = index * 40
    else:
      ypos = 51
      xpos = (index-4) * 40

    # command value not available in Python
    v1 = 0

    # The actual velocity of the motor in rpm
    v2 = m.velocity(RPM)

    # The position of the motor internal encoder in revolutions
    pos = m.position(TURNS)

    # Motor current in Amps
    c1 = m.current()

    brain.screen.set_font(FontType.MONO12)

    # background color based on
    # device and whether it's left, right or other motor
    if not m.installed():
        brain.screen.set_fill_color(grey)
    elif m == left_drive_1 or m == left_drive_2:
        brain.screen.set_fill_color(lblue)
    elif m == right_drive_1 or m == right_drive_2:
        brain.screen.set_fill_color(lred)
    else:
        brain.screen.set_fill_color(dgrey)

    # Draw outline for motor info
    brain.screen.set_pen_color(Color.WHITE)
    w = 41 if xpos < 120 else 40
    brain.screen.draw_rectangle(xpos, ypos, w, 52)

    # no motor, then return early
    if not m.installed():
        brain.screen.print_at("P %d" % (m.index()+1), x=xpos+3, y=ypos+11)
        brain.screen.print_at("NC", x=xpos+15, y=ypos+30)
        return

    # we have no way to get command value in Python VM
    # so have to deviate from C++ version, just show port number
    brain.screen.print_at("P %d" % (m.index()+1), x=xpos+3, y=ypos+11)

    # Show actual speed
    brain.screen.set_pen_color(Color.YELLOW)
    brain.screen.print_at("%4d" % v2, x=xpos+6, y=ypos+24)

    # Show position
    brain.screen.print_at("%5.1f" % pos, x=xpos+1, y=ypos+36)

    # Show current
    brain.screen.print_at("%4.1fA" % c1, x=xpos+1, y=ypos+48)

    brain.screen.set_pen_color(Color.WHITE)
    brain.screen.draw_line(xpos, ypos+14, xpos+40, ypos+14)


#-----------------------------------------------------------------------------*/
#   @brief  Display task - show some useful motor data                        */
#-----------------------------------------------------------------------------*/
def display_task():
    #brain.screen.set_font(FontType.PROP40)
    #brain.screen.set_pen_color(Color.RED)
    #brain.screen.print_at("TEST DRIVE CODE", x=90, y=160)

    motors = [motor_1,
              motor_4,
              motor_5,
              motor_6,
              motor_7,
              motor_10,
              motor_11,
              motor_12]

    while True:
        index = 0
        for m in motors:
            displayMotorData(m, index)
            index = index+1

        # synchronize with display, stops flickering
        brain.screen.render()

# Run the drive code
drive = Thread(drive_task)

# Run the display code
display = Thread(display_task)

# Python now drops into REPL

both C++ and Python versions are in this zip archive, I have no plans to do a blocks version, too much hard work to program in blocks.

IQ2_default.zip (6.5 KB)

10 Likes

:wink:

This is a great idea! I always like the idea of making debugging easier.

3 Likes