def __init__(self):
self.brain = Brain()
self.left_motor = \
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.right_motor = \
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.drivetrain = \
Drivetrain(
self.left_motor, # left_motor
self.right_motor, # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def __init__(self):
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse
), # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse
)
class Slick:
# Drive Base configs
LEFT_MOTOR_PORT = Ports.PORT1
LEFT_MOTOR_REVERSE_POLARITY = False
RIGHT_MOTOR_PORT = Ports.PORT6
RIGHT_MOTOR_REVERSE_POLARITY = True
WHEEL_TRAVEL = 200
TRACK_WIDTH = 176
DISTANCE_UNIT = DistanceUnits.MM
GEAR_RATIO = 1
# Controller configs
CONTROLLER_DEADBAND = 3
def __init__(self):
self.brain = Brain()
self.left_motor = \
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.right_motor = \
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.drivetrain = \
Drivetrain(
self.left_motor, # left_motor
self.right_motor, # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def drive_by_controller(self):
self.left_motor.spin(
DirectionType.FWD, # dir
self.controller.axisA.position(), # velocity
VelocityUnits.PCT # velocityUnit
)
self.right_motor.spin(
DirectionType.FWD, # dir
self.controller.axisD.position(), # velocity
VelocityUnits.PCT # velocityUnit
)
def keep_driving_by_controller(self):
while True:
self.drive_by_controller()
def main(self):
self.keep_driving_by_controller()
def __init__(self):
self.brain = Brain()
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse
), # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.touch_led = Touchled(self.TOUCH_LED_PORT)
self.color_sensor = \
Colorsensor(
self.COLOR_SENSOR_PORT, # index
False, # is_grayscale
700 # proximity
)
self.gyro_sensor = \
Gyro(
self.GYRO_SENSOR_PORT, # index
True # calibrate
)
self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT)
class Clawbot:
# drive base configs
LEFT_MOTOR_PORT = Ports.PORT1
LEFT_MOTOR_REVERSE_POLARITY = False
RIGHT_MOTOR_PORT = Ports.PORT6
RIGHT_MOTOR_REVERSE_POLARITY = True
WHEEL_TRAVEL = 200
TRACK_WIDTH = 176
DISTANCE_UNIT = DistanceUnits.MM
GEAR_RATIO = 1
# actuator configs
ARM_MOTOR_PORT = Ports.PORT10
ARM_MOTOR_REVERSE_POLARITY = False
ARM_MOTOR_VELOCITY = 30 # %
CLAW_MOTOR_PORT = Ports.PORT11
CLAW_MOTOR_REVERSE_POLARITY = False
CLAW_MOTOR_TIMEOUT_SECS = 3
CLAW_MOTOR_VELOCITY = 60 # %
def __init__(self):
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse
), # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse
)
self.claw_motor.set_timeout(
self.CLAW_MOTOR_TIMEOUT_SECS, # time
TimeUnits.SEC # timeUnit
)
def __init__(self):
self.brain = Brain()
self.left_motor = \
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.right_motor = \
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.drivetrain = \
Drivetrain(
self.left_motor, # left_motor
self.right_motor, # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor.set_timeout(
self.CLAW_MOTOR_TIMEOUT_SECS, # time
TimeUnits.SEC # timeUnit
)
self.touch_led = Touchled(self.TOUCH_LED_PORT)
self.color_sensor = \
Colorsensor(
self.COLOR_SENSOR_PORT, # index
False, # is_grayscale
700 # proximity
)
self.gyro_sensor = \
Gyro(
self.GYRO_SENSOR_PORT, # index
True # calibrate
)
self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT)
self.bumper_switch = Bumper(self.BUMPER_SWITCH_PORT)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def __init__(self):
self.brain = Brain()
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse
)
self.control_arm_or_claw = 0
def __init__(self):
self.brain = Brain()
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse
)
def __init__(self):
self.brain = Brain()
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse
)
def __init__(self):
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse
), # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.bumper_switch = Bumper(self.BUMPER_SWITCH_PORT)
def __init__(self):
self.brain = Brain()
self.base_pivot_motor = \
Motor(
self.BASE_PIVOT_MOTOR_PORT, # index
self.BASE_PIVOT_REVERSE_POLARITY # reverse polarity?
)
self.shoulder_motor = \
Motor(
self.SHOULDER_MOTOR_PORT, # index
self.SHOULDER_REVERSE_POLARITY # reverse polarity?
)
self.elbow_motor = \
Motor(
self.ELBOW_MOTOR_PORT, # index
self.ELBOW_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor.set_timeout(
self.CLAW_MOTOR_TIMEOUT_SECS, # time
TimeUnits.SEC # timeUnit
)
self.base_bumper_switch = Bumper(self.BASE_BUMPER_SWITCH_PORT)
self.elbow_bumper_switch = Bumper(self.ELBOW_BUMPER_SWITCH_PORT)
self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT)
self.color_sensor = \
Colorsensor(
self.COLOR_SENSOR_PORT, # index
False, # is_grayscale
700 # proximity
)
self.left_touch_led = Touchled(self.LEFT_TOUCH_LED_PORT)
self.right_touch_led = Touchled(self.RIGHT_TOUCH_LED_PORT)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def __init__(self):
self.front_left_motor = \
Motor(
self.FRONT_LEFT_MOTOR_PORT, # index
self.FRONT_LEFT_MOTOR_REVERSE_POLARITY # reverse
)
self.front_right_motor = \
Motor(
self.FRONT_RIGHT_MOTOR_PORT, # index
self.FRONT_RIGHT_MOTOR_REVERSE_POLARITY # reverse
)
self.rear_left_motor = \
Motor(
self.REAR_LEFT_MOTOR_PORT, # index
self.REAR_LEFT_MOTOR_REVERSE_POLARITY # reverse
)
self.rear_right_motor = \
Motor(
self.REAR_RIGHT_MOTOR_PORT, # index
self.REAR_RIGHT_MOTOR_REVERSE_POLARITY # reverse
)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
DistanceUnits,
Gyro,
Motor,
Ports,
Sonar,
TimeUnits,
Touchled,
VelocityUnits
)
BRAIN = Brain()
# Drive Base configs
LEFT_MOTOR = Motor(Ports.PORT1, # index
False # reverse polarity?
)
RIGHT_MOTOR = Motor(Ports.PORT6, # index
True # reverse polarity?
)
DRIVETRAIN = Drivetrain(LEFT_MOTOR, # left_motor
RIGHT_MOTOR, # right_motor
200, # wheel_travel
176, # track_width
DistanceUnits.MM, # distanceUnit
1 # gear_ratio
)
# Arm motor configs
ARM_MOTOR = Motor(Ports.PORT10, # index
False # reverse
Bumper,
Touchled,
Ports,
DistanceUnits,
DirectionType,
ColorHue
)
from vex import (REVERSE, LEFT)
from drivetrain import Drivetrain
# KHỞI TẠO CÁC BỘ PHẬN ROBOT
# ==========================
# khởi tạo các motor, cắm vào cổng 1 của Brain (bộ xử lý trung tâm)
motor_right = Motor(Ports.PORT1, True) # Reverse Polarity
motor_left = Motor(Ports.PORT2)
# khởi tạo drivetrain (bộ truyền động) với 2 motor
# mỗi motor gắn một bánh xe chu vi 200mm
# và khoảng cách giữa 2 bánh xe 2 bên là 202mm
# tỉ lệ bánh răng 1:1
drivetrain = Drivetrain(
motor_left, motor_right,
200, 200, DistanceUnits.MM, 1)
# khởi tạo đèn LED cảm ứng
touch_led = Touchled(Ports.PORT10)
# khởi tạo bumper trước
bumper_front = Bumper(Ports.PORT3)
"""
Unit Test này yêu cầu mô hình Slick có gắn thêm Motors & Sensors
On Robot Mesh Studio: https://www.robotmesh.com/studio/5ffe521a7ec24d06520134c2
"""
# NHẬP CÁC ĐỐI TƯỢNG TỪ THƯ VIỆN
# ==============================
from vex import (Motor, Ports, DEGREES, FORWARD, PERCENT, REVERSE)
# KHỞI TẠO CÁC BỘ PHẬN ROBOT
# ==========================
# khởi tạo các motor
motor_1 = Motor(Ports.PORT1, True) # reverse: đảo ngược chiều xoay mặc định
motor_6 = Motor(Ports.PORT6)
motor_7 = Motor(Ports.PORT7, True) # reverse: đảo ngược chiều xoay mặc định
motor_12 = Motor(Ports.PORT12)
# CHƯƠNG TRÌNH CHÍNH
# ==================
# lần lượt quay các motor
for motor in [motor_1, motor_6, motor_7, motor_12]:
motor.spin_for(
FORWARD, # quay theo chiều xuôi
360,
DEGREES, # 360 độ (1 vòng)
100,
PERCENT, # tốc độ (velocity) 100%
from vex import (Brain, BrakeType, Controller, Motor, Ports, FORWARD, REVERSE,
PERCENT, SECONDS)
# CONFIGURE ROBOT PARTS
# =====================
# Brain
BRAIN = Brain()
# Base Pivot motor
BASE_PIVOT_MOTOR = Motor(Ports.PORT10)
# Shoulder motor
SHOULDER_MOTOR = Motor(
Ports.PORT6,
True # reverse polarity?
)
# Elbow motor
ELBOW_MOTOR = Motor(Ports.PORT1)
# Claw motor
CLAW_MOTOR = Motor(Ports.PORT4)
CLAW_MOTOR.set_timeout(3, SECONDS)
# Controller
CONTROLLER = Controller()
CONTROLLER.set_deadband(3)
# FUNCTIONS
# =========
#region config
đến
#endregion config
"""
# NẠP CÁC ĐỐI TƯỢNG TỪ THƯ VIỆN
# ================================
from vex import Motor, Ports
from vex import DirectionType, DistanceUnits
from drivetrain import Drivetrain
# KHỞI TẠO CÁC BỘ PHẬN ROBOT
# ==========================
# khởi tạo các motor tương ứng với các cổng
motor_left = Motor(Ports.PORT1, True)
motor_right = Motor(Ports.PORT2)
# khởi tạo bộ truyền động
drive_train = Drivetrain(motor_left, motor_right,
200, 192, DistanceUnits.MM, 1)
# CHƯƠNG TRÌNH CHÍNH
# ==========================
# di chuyển robot đến phía trước 120cm, tốc độ 50%
drive_train.drive_for(DirectionType.FWD, 120, DistanceUnits.CM, 50)
# di chuyển robot lui về phía sau 120cm, tốc độ 100%
drive_train.drive_for(DirectionType.REV, 120, DistanceUnits.CM, 100)
class Armbot:
# Base Pivot motor configs
BASE_PIVOT_MOTOR_PORT = Ports.PORT10
BASE_PIVOT_REVERSE_POLARITY = False
BASE_PIVOT_VELOCITY_PERCENT = 100
# Shoulder motor configs
SHOULDER_MOTOR_PORT = Ports.PORT6
SHOULDER_REVERSE_POLARITY = True # reverse polarity
# Elbow motor configs
ELBOW_MOTOR_PORT = Ports.PORT1
ELBOW_REVERSE_POLARITY = False
# Claw motor configs
CLAW_MOTOR_PORT = Ports.PORT4
CLAW_MOTOR_REVERSE_POLARITY = False
CLAW_MOTOR_VELOCITY_PERCENT = 100
CLAW_MOTOR_TIMEOUT_SECS = 3
# sensor configs
BASE_BUMPER_SWITCH_PORT = Ports.PORT2
ELBOW_BUMPER_SWITCH_PORT = Ports.PORT9
DISTANCE_SENSOR_PORT = Ports.PORT8
COLOR_SENSOR_PORT = Ports.PORT12
LEFT_TOUCH_LED_PORT = Ports.PORT5
RIGHT_TOUCH_LED_PORT = Ports.PORT11
# controller configs
CONTROLLER_DEADBAND = 3 # seconds
def __init__(self):
self.brain = Brain()
self.base_pivot_motor = \
Motor(
self.BASE_PIVOT_MOTOR_PORT, # index
self.BASE_PIVOT_REVERSE_POLARITY # reverse polarity?
)
self.shoulder_motor = \
Motor(
self.SHOULDER_MOTOR_PORT, # index
self.SHOULDER_REVERSE_POLARITY # reverse polarity?
)
self.elbow_motor = \
Motor(
self.ELBOW_MOTOR_PORT, # index
self.ELBOW_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor.set_timeout(
self.CLAW_MOTOR_TIMEOUT_SECS, # time
TimeUnits.SEC # timeUnit
)
self.base_bumper_switch = Bumper(self.BASE_BUMPER_SWITCH_PORT)
self.elbow_bumper_switch = Bumper(self.ELBOW_BUMPER_SWITCH_PORT)
self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT)
self.color_sensor = \
Colorsensor(
self.COLOR_SENSOR_PORT, # index
False, # is_grayscale
700 # proximity
)
self.left_touch_led = Touchled(self.LEFT_TOUCH_LED_PORT)
self.right_touch_led = Touchled(self.RIGHT_TOUCH_LED_PORT)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def pivot_base_by_controller_left_buttons(self):
# pivot base right
if self.controller.buttonLDown.pressing():
self.base_pivot_motor.spin(
DirectionType.REV, # dir
self.BASE_PIVOT_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
# pivot base left
elif self.controller.buttonLUp.pressing():
self.base_pivot_motor.spin(
DirectionType.FWD, # dir
self.BASE_PIVOT_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.base_pivot_motor.stop(BrakeType.HOLD)
def keep_pivoting_base_by_controller_left_buttons(self):
while True:
self.pivot_base_by_controller_left_buttons()
def control_shoulder_by_controller_axis_d(self):
controller_axis_d_position = self.controller.axisD.position()
if ((controller_axis_d_position > 0) and
(not self.base_bumper_switch.pressing())) or \
(controller_axis_d_position < 0):
self.shoulder_motor.spin(
DirectionType.FWD, # dir
controller_axis_d_position, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.shoulder_motor.stop(BrakeType.HOLD)
def keep_controlling_shoulder_by_controller_axis_d(self):
while True:
self.control_shoulder_by_controller_axis_d()
def control_elbow_by_controller_axis_a(self):
controller_axis_a_position = self.controller.axisA.position()
if ((controller_axis_a_position > 0) and
(not self.elbow_bumper_switch.pressing())) or \
(controller_axis_a_position < 0):
self.elbow_motor.spin(
DirectionType.FWD, # dir
controller_axis_a_position, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.elbow_motor.stop(BrakeType.HOLD)
def keep_controlling_elbow_by_controller_axis_a(self):
while True:
self.control_elbow_by_controller_axis_a()
def control_claw_by_controller_right_buttons(self):
# open claw
if self.controller.buttonRDown.pressing():
self.claw_motor.spin(
DirectionType.REV, # dir
self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
# close claw
elif self.controller.buttonRUp.pressing():
self.claw_motor.spin(
DirectionType.FWD, # dir
self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.claw_motor.stop(BrakeType.HOLD)
def keep_controlling_claw_by_controller_right_buttons(self):
while True:
self.control_claw_by_controller_right_buttons()
def main(self):
while True:
self.pivot_base_by_controller_left_buttons()
self.control_shoulder_by_controller_axis_d()
self.control_elbow_by_controller_axis_a()
self.control_claw_by_controller_right_buttons()
from vex import (
Gyro,
Motor,
Ports,
FORWARD,
LEFT,
RIGHT,
Sonar # Sonar = Cảm biến khoảng cách
)
from drivetrain import Drivetrain
# KHỞI TẠO CÁC BỘ PHẬN ROBOT
# ==========================
# khởi tạo các motor
motor_right = Motor(Ports.PORT1, True) # reverse=True: đảo ngược chiều xoay
motor_left = Motor(Ports.PORT2)
# khởi tạo drivetrain với 2 motor, mỗi motor gắn một bánh xe
# chu vi 200mm và khoảng cách giữa 2 bánh xe 2 bên là 198mm
dt = Drivetrain(motor_left, motor_right, 200, 198)
# khởi tạo cảm biến khoảng cách
distance_sensor = Sonar(Ports.PORT7)
# khởi tạo cảm biến Gyro (phương hướng)
gyro = Gyro(Ports.PORT3)
# ĐỊNH NGHĨA HẰNG SỐ
# ===========================
from sys import run_in_thread
from vex import (Brain, BrakeType, Bumper, Colorsensor, Controller,
DirectionType, Motor, Ports, Sonar, TimeUnits, Touchled,
VelocityUnits)
BRAIN = Brain()
# Base Pivot motor configs
BASE_PIVOT_MOTOR = Motor(
Ports.PORT10, # index
False # reverse polarity?
)
BASE_PIVOT_VELOCITY_PERCENT = 100
# Shoulder motor configs
SHOULDER_MOTOR = Motor(
Ports.PORT6, # index
True # reverse polarity?
)
# Elbow motor configs
ELBOW_MOTOR = Motor(
Ports.PORT1, # index
False # reverse polarity?
)
# Claw motor configs
CLAW_MOTOR = Motor(
Ports.PORT4, # index
False # reverse polarity?
class Clawbot:
# Drive Base configs
LEFT_MOTOR_PORT = Ports.PORT1
LEFT_MOTOR_REVERSE_POLARITY = False
RIGHT_MOTOR_PORT = Ports.PORT6
RIGHT_MOTOR_REVERSE_POLARITY = True
WHEEL_TRAVEL = 200
TRACK_WIDTH = 176
DISTANCE_UNIT = DistanceUnits.MM
GEAR_RATIO = 1
# Arm motor configs
ARM_MOTOR_PORT = Ports.PORT10
ARM_MOTOR_REVERSE_POLARITY = False
ARM_MOTOR_VELOCITY_PERCENT = 30
# Claw motor configs
CLAW_MOTOR_PORT = Ports.PORT11
CLAW_MOTOR_REVERSE_POLARITY = False
CLAW_MOTOR_TIMEOUT_SECS = 3
CLAW_MOTOR_VELOCITY_PERCENT = 60
# sensor configs
TOUCH_LED_PORT = Ports.PORT2
COLOR_SENSOR_PORT = Ports.PORT3
GYRO_SENSOR_PORT = Ports.PORT4
DISTANCE_SENSOR_PORT = Ports.PORT7
BUMPER_SWITCH_PORT = Ports.PORT8
# Controller configs
CONTROLLER_DEADBAND = 3
def __init__(self):
self.brain = Brain()
self.left_motor = \
Motor(
self.LEFT_MOTOR_PORT, # index
self.LEFT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.right_motor = \
Motor(
self.RIGHT_MOTOR_PORT, # index
self.RIGHT_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.drivetrain = \
Drivetrain(
self.left_motor, # left_motor
self.right_motor, # right_motor
self.WHEEL_TRAVEL, # wheel_travel
self.TRACK_WIDTH, # track_width
self.DISTANCE_UNIT, # distanceUnit
self.GEAR_RATIO # gear_ratio
)
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse polarity?
)
self.claw_motor.set_timeout(
self.CLAW_MOTOR_TIMEOUT_SECS, # time
TimeUnits.SEC # timeUnit
)
self.touch_led = Touchled(self.TOUCH_LED_PORT)
self.color_sensor = \
Colorsensor(
self.COLOR_SENSOR_PORT, # index
False, # is_grayscale
700 # proximity
)
self.gyro_sensor = \
Gyro(
self.GYRO_SENSOR_PORT, # index
True # calibrate
)
self.distance_sensor = Sonar(self.DISTANCE_SENSOR_PORT)
self.bumper_switch = Bumper(self.BUMPER_SWITCH_PORT)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def drive_by_controller(self):
self.left_motor.spin(
DirectionType.FWD, # dir
self.controller.axisA.position(), # velocity
VelocityUnits.PCT # velocityUnit
)
self.right_motor.spin(
DirectionType.FWD, # dir
self.controller.axisD.position(), # velocity
VelocityUnits.PCT # velocityUnit
)
def keep_driving_by_controller(self):
while True:
self.drive_by_controller()
def lower_or_raise_arm_by_controller(self):
if self.controller.buttonLDown.pressing():
self.arm_motor.spin(
DirectionType.REV, # dir
self.ARM_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
elif self.controller.buttonLUp.pressing():
self.arm_motor.spin(
DirectionType.FWD, # dir
self.ARM_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.arm_motor.stop(BrakeType.HOLD)
def keep_lowering_or_raising_arm_by_controller(self):
while True:
self.lower_or_raise_arm_by_controller()
def grab_or_release_object_by_controller(self):
if self.controller.buttonRDown.pressing():
self.claw_motor.spin(
DirectionType.REV, # dir
self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
elif self.controller.buttonRUp.pressing():
self.claw_motor.spin(
DirectionType.FWD, # dir
self.CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.claw_motor.stop(BrakeType.HOLD)
def keep_grabbing_or_releasing_objects_by_controller(self):
while True:
self.grab_or_release_object_by_controller()
def main(self):
while True:
self.drive_by_controller()
self.lower_or_raise_arm_by_controller()
self.grab_or_release_object_by_controller()
class Clawbot:
ARM_MOTOR_PORT = Ports.PORT10
ARM_MOTOR_REVERSE_POLARITY = False
CLAW_MOTOR_PORT = Ports.PORT11
CLAW_MOTOR_REVERSE_POLARITY = False
def __init__(self):
self.brain = Brain()
self.arm_motor = \
Motor(
self.ARM_MOTOR_PORT, # index
self.ARM_MOTOR_REVERSE_POLARITY # reverse
)
self.claw_motor = \
Motor(
self.CLAW_MOTOR_PORT, # index
self.CLAW_MOTOR_REVERSE_POLARITY # reverse
)
self.control_arm_or_claw = 0
def switch_between_arm_and_claw(self):
if self.brain.buttonCheck.pressing():
self.control_arm_or_claw = 1 - self.control_arm_or_claw
def control_arm(self):
if not self.control_arm_or_claw:
if self.brain.buttonUp.pressing():
self.arm_motor.spin(
DirectionType.FWD, # dir
100, # velocity
VelocityUnits.PCT # velocityUnit
)
elif self.brain.buttonDown.pressing():
self.arm_motor.spin(
DirectionType.REV, # dir
100, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.arm_motor.stop(BrakeType.HOLD)
else:
self.arm_motor.stop(BrakeType.HOLD)
def control_claw(self):
if self.control_arm_or_claw:
if self.brain.buttonDown.pressing():
self.claw_motor.spin(
DirectionType.REV, # dir
None, # velocity
VelocityUnits.PCT # velocityUnit
)
elif self.brain.buttonUp.pressing():
self.claw_motor.spin(
DirectionType.FWD, # dir
None, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
self.claw_motor.stop(BrakeType.HOLD)
else:
self.claw_motor.stop(BrakeType.HOLD)
class Allie:
FRONT_LEFT_MOTOR_PORT = Ports.PORT1
FRONT_LEFT_MOTOR_REVERSE_POLARITY = False
FRONT_RIGHT_MOTOR_PORT = Ports.PORT6
FRONT_RIGHT_MOTOR_REVERSE_POLARITY = True
REAR_LEFT_MOTOR_PORT = Ports.PORT7
REAR_LEFT_MOTOR_REVERSE_POLARITY = False
REAR_RIGHT_MOTOR_PORT = Ports.PORT12
REAR_RIGHT_MOTOR_REVERSE_POLARITY = True
MOTOR_VELOCITY_PCT = 100
CONTROLLER_DEADBAND = 3 # seconds
def __init__(self):
self.front_left_motor = \
Motor(
self.FRONT_LEFT_MOTOR_PORT, # index
self.FRONT_LEFT_MOTOR_REVERSE_POLARITY # reverse
)
self.front_right_motor = \
Motor(
self.FRONT_RIGHT_MOTOR_PORT, # index
self.FRONT_RIGHT_MOTOR_REVERSE_POLARITY # reverse
)
self.rear_left_motor = \
Motor(
self.REAR_LEFT_MOTOR_PORT, # index
self.REAR_LEFT_MOTOR_REVERSE_POLARITY # reverse
)
self.rear_right_motor = \
Motor(
self.REAR_RIGHT_MOTOR_PORT, # index
self.REAR_RIGHT_MOTOR_REVERSE_POLARITY # reverse
)
self.controller = Controller()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def rest_leg_on_surface(self, leg_motor, forward=True):
# rotate motor up to 270 degrees within 1 second
# using max velocity but very light power/torque
leg_motor.set_max_torque_percent(5)
leg_motor.set_timeout(
1, # time,
TimeUnits.SEC # timeUnit
)
leg_motor.spin_for(
DirectionType.FWD if forward else DirectionType.REV, # dir
270, # rotation
RotationUnits.DEG, # rotationUnit
100, # velocity
VelocityUnits.PCT, # velocityUnit
True # waitForCompletion
)
# restore max motor power/torque to 100% again
leg_motor.set_max_torque_percent(100)
def reset_legs(self):
for leg_motor in (self.front_left_motor, self.front_right_motor,
self.rear_left_motor, self.rear_right_motor):
self.rest_leg_on_surface(leg_motor, True)
# set motor encoder to 0
leg_motor.reset_rotation()
def rest_left_legs_forward(self):
for leg_motor in (self.front_left_motor, self.rear_left_motor):
self.rest_leg_on_surface(leg_motor, True)
def rest_left_legs_backward(self):
for leg_motor in (self.rear_left_motor, self.front_left_motor):
self.rest_leg_on_surface(leg_motor, False)
def rest_right_legs_forward(self):
for leg_motor in (self.front_right_motor, self.rear_right_motor):
self.rest_leg_on_surface(leg_motor, True)
def rest_right_legs_backward(self):
for leg_motor in (self.rear_right_motor, self.front_right_motor):
self.rest_leg_on_surface(leg_motor, False)
def remote_control_once(self):
if self.controller.buttonEUp.pressing():
self.rest_left_legs_forward()
elif self.controller.buttonEDown.pressing():
self.rest_left_legs_backward()
elif self.controller.buttonFUp.pressing():
self.rest_right_legs_forward()
elif self.controller.buttonFDown.pressing():
self.rest_right_legs_backward()
else:
left_velocity = self.controller.axisA.position()
right_velocity = self.controller.axisD.position()
self.front_left_motor.spin(
DirectionType.FWD, # dir
left_velocity, # velocity
VelocityUnits.PCT # velocityUnit
)
self.rear_left_motor.spin(
DirectionType.FWD, # dir
left_velocity, # velocity
VelocityUnits.PCT # velocityUnit
)
self.front_right_motor.spin(
DirectionType.FWD, # dir
right_velocity, # velocity
VelocityUnits.PCT # velocityUnit
)
self.rear_right_motor.spin(
DirectionType.FWD, # dir
right_velocity, # velocity
VelocityUnits.PCT # velocityUnit
)
def keep_remote_controlling(self):
while True:
self.remote_control_once()
