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.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.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)
CLAW_MOTOR_VELOCITY_PERCENT = 60
# sensor configs
TOUCH_LED = Touchled(Ports.PORT2)
COLOR_SENSOR = Colorsensor(Ports.PORT3, # index
False, # is_grayscale
700 # proximity
)
GYRO_SENSOR = Gyro(Ports.PORT4, # index
True # calibrate
)
DISTANCE_SENSOR = Sonar(Ports.PORT7)
BUMPER_SWITCH = Bumper(Ports.PORT8)
# Controller configs
CONTROLLER = Controller()
CONTROLLER.set_deadband(3)
def drive_by_controller():
LEFT_MOTOR.spin(
DirectionType.FWD, # dir
CONTROLLER.axisA.position(), # velocity
VelocityUnits.PCT # velocityUnit
)
RIGHT_MOTOR.spin(
DirectionType.FWD, # dir
CONTROLLER.axisD.position(), # velocity
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)
# ĐỊNH NGHĨA HẰNG SỐ, THAM SỐ
# ===========================
robot_started = False
drivetrain.set_drive_velocity(100)
# ĐỊNH NGHĨA CÁC HÀM
# ==================
def flash_led(color, duration_in_seconds):
touch_led.on_hue(color)
sys.sleep(duration_in_seconds)
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()
# Elbow motor configs
ELBOW_MOTOR = Motor(
Ports.PORT1, # index
False # reverse polarity?
)
# Claw motor configs
CLAW_MOTOR = Motor(
Ports.PORT4, # index
False # reverse polarity?
)
CLAW_MOTOR.set_timeout(3, TimeUnits.SEC)
CLAW_MOTOR_VELOCITY_PERCENT = 100
# sensor configs
BASE_BUMPER_SWITCH = Bumper(Ports.PORT2)
ELBOW_BUMPER_SWITCH = Bumper(Ports.PORT9)
DISTANCE_SENSOR = Sonar(Ports.PORT8)
COLOR_SENSOR = Colorsensor(
Ports.PORT12, # index
False, # is_grayscale
700 # proximity
)
LEFT_TOUCH_LED = Touchled(Ports.PORT5)
RIGHT_TOUCH_LED = Touchled(Ports.PORT12)
# Controller configs
CONTROLLER = Controller()
from vex import (
Brain, Bumper,
Ports, SECONDS
)
from random import randint
# KHỞI TẠO CÁC BỘ PHẬN ROBOT
# ==========================
# khởi tạo brain
brain = Brain()
# khởi tạo các bumper switch
bumper_9 = Bumper(Ports.PORT9)
bumper_10 = Bumper(Ports.PORT10)
# CHƯƠNG TRÌNH CHÍNH
# ==================
while True:
# mỗi khi một trong những bumper switches được nhấn
# thì brain phát âm thanh ngẫu nhiên
if bumper_9.pressing() or bumper_10.pressing():
brain.sound.play(
randint(1, 7), # note: một trong 7 note C, D, E, F, G, A, B
randint(1, 7), # octave: một số từ 1 đến 7
0.5, SECONDS # 0.5 giây
)
PLAY_TIME_IN_SECONDS = 60 # KHỞI TẠO CÁC BỘ PHẬN ROBOT # ========================== # khởi tạo brain brain = Brain() # khởi tạo các motor motor_1 = Motor(Ports.PORT1, True) # reverse=True: đảo ngược chiều xoay motor_6 = Motor(Ports.PORT6) motor_7 = Motor(Ports.PORT7, True) # reverse=True: đảo ngược chiều xoay motor_12 = Motor(Ports.PORT12) # khởi tạo các bumper switch bumper_9 = Bumper(Ports.PORT9) bumper_10 = Bumper(Ports.PORT10) # khởi tạo sensor cảm nhận màu sắc color_sensor = Colorsensor(Ports.PORT2) # sensor cảm nhận phương hướng gyro = Gyro(Ports.PORT3) # khởi tạo sensor cảm nhận khoảng cách distance_sensor = Sonar(Ports.PORT4) # khởi tạo các đèn LED cảm nhận chạm touch_led_8 = Touchled(Ports.PORT8) touch_led_8.brightness(100) touch_led_8.default_fade(FadeType.OFF)
# ==========================
# khởi tạo các motor, cắm vào cổng 1 & 2 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, 202, DistanceUnits.MM)
# khởi tạo đèn LED cảm ứng
touch_led = Touchled(Ports.PORT10)
# cảm biến va chạm phía trước
bumper_front = Bumper(Ports.PORT6)
# khởi tạo trạng thái
state = "IDLE_STATE"
# Định nghĩa các trạng thái
def idle_state():
# Lấy biến trạng thái toàn cục
global state
# print "Idle state"
drivetrain.stop()
touch_led.on_hue(ColorHue.RED)
while True: