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.
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)
