def robotInit(self):
self.drivetrain = Drivetrain(18, self)
self.fourbar = Fourbar(self)
self.beak = Beak(self)
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
self.timer = wpilib.Timer()
self.loops = 0
wpilib.CameraServer.launch("wision.py:main")
def robotInit(self):
print('robotinit')
self.slowMode = True
self.drivetrain = Drivetrain()
self.elevator = Elevator()
self.driver = wpilib.XboxController(JOYSTICK_PORT)
self.intake = Intake()
self.limelight = CitrusLumen()
self.autonTimer = wpilib.Timer()
self.autonCase = None
self.autonAbort = None
self.debugging = False
def robotInit(self):
# Instances of classes
# Instantiate Subsystems
self.drivetrain = Drivetrain(self)
self.hp_intake = Hp_Intake()
self.ramp = Ramp()
self.shifters = Shifters()
# instantiate Encoders for drivetrain?
#self.encoders = Encoders(self)
# Instantiate Joysticks
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
# Instantiate Xbox
self.xbox = wpilib.Joystick(2)
# Instantiate OI; must be AFTER joysticks are inited
self.oi = OI(self)
self.timer = wpilib.Timer()
self.loops = 0
# untested vision
#XXX might crash sim
wpilib.CameraServer.launch("vision.py:main")
def __init__(self):
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # port
self.LEFT_MOTOR_REVERSE_POLARITY # switch_polarity
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # port
self.RIGHT_MOTOR_REVERSE_POLARITY # switch_polarity
), # right_motor
self.WHEEL_TRAVEL_MM, # wheel_travel_mm
self.TRACK_MM # track_mm
)
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 = \
DistanceSensor(
self.DISTANCE_SENSOR_PORT, # port
self.DISTANCE_SENSOR_UNIT # unit
)
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.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
)
def __init__(self):
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # port
self.LEFT_MOTOR_REVERSE_POLARITY # switch_polarity
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # port
self.RIGHT_MOTOR_REVERSE_POLARITY # switch_polarity
), # right_motor
self.WHEEL_TRAVEL_MM, # wheel_travel_mm
self.TRACK_MM # track_mm
)
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
)
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)
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 robotInit(self):
super().__init__()
# Instances of classes
# Instantiate Subsystems
#XXX DEBUGGING
self.drivetrain = Drivetrain(self)
self.shooter = Shooter(self)
self.carrier = Carrier(self)
self.feeder = Feeder(self)
self.intake = Intake(self)
#self.winch = Winch(self)
#self.climber = Climber(self)
#self.climber_big = Climber_Big(self)
#self.climber_little = Climber_Little(self)
# Instantiate Joysticks
self.left_joy = wpilib.Joystick(1)
self.right_joy = wpilib.Joystick(2)
# Instantiate Xbox
self.xbox = wpilib.Joystick(3)
# Instantiate OI; must be AFTER joysticks are inited
self.oi = OI(self)
self.timer = wpilib.Timer()
class BeaverTronicsRobot(wpilib.TimedRobot):
def robotInit(self):
self.drivetrain = Drivetrain(18, self)
self.fourbar = Fourbar(self)
self.beak = Beak(self)
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
self.timer = wpilib.Timer()
self.loops = 0
wpilib.CameraServer.launch("wision.py:main")
def autonomousInit(self):
Sheduler.getInstance().removeAll()
data = wpilib.DriverStation.getInstane().getGameSpecificMessage()
self.do_2_hatch_panel_auto_left.start()
self.drivetrain.shift_low()
def autonomousPeriodic(self):
Scheduler.getInstance().run()
def teleopInit(self):
self.loops = 0
self.timer.reset()
self.timer.start()
Scheduler.getInstance().removeAll()
Scheduler.getInstance().enable()
def teleopPeriodic(self):
Scheduler.getInstance().run()
speed = self.right_joy.getY()
rotation = self.left_joy.getX()
self.drivetrain.drive.arcadeDrive(speed, rotation, True)
self.loops += 1
if self.timer.hasPeriodPassed(1):
self.logger.info("%d loops / second", self.loops)
self.loops = 0
def disabledInit(self):
self.drivetrain.stop()
Scheduler.getInstance().removeAll()
return None
def disabledPeriodic(self):
return None
def __init__(self):
self.left_motor = \
Motor(
self.LEFT_MOTOR_PORT, # port
self.LEFT_MOTOR_REVERSE_POLARITY # switch_polarity
)
self.right_motor = \
Motor(
self.RIGHT_MOTOR_PORT, # port
self.RIGHT_MOTOR_REVERSE_POLARITY # switch_polarity
)
self.drivetrain = \
Drivetrain(
self.left_motor, # left_motor
self.right_motor, # right_motor
self.WHEEL_TRAVEL_MM, # wheel_travel_mm
self.TRACK_MM # track_mm
)
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.stall_timeout = self.CLAW_MOTOR_TIMEOUT_SECS
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 = \
DistanceSensor(
self.DISTANCE_SENSOR_PORT, # port
self.DISTANCE_SENSOR_UNIT # unit
)
self.bumper_switch = Bumper(self.BUMPER_SWITCH_PORT)
self.controller = Joystick()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
def __init__(self):
self.drivetrain = \
Drivetrain(
Motor(
self.LEFT_MOTOR_PORT, # port
self.LEFT_MOTOR_REVERSE_POLARITY # switch_polarity
), # left_motor
Motor(
self.RIGHT_MOTOR_PORT, # port
self.RIGHT_MOTOR_REVERSE_POLARITY # switch_polarity
), # right_motor
self.WHEEL_TRAVEL_MM, # wheel_travel_mm
self.TRACK_MM # track_mm
)
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 robotInit(self):
super().__init__()
# gc.collect()
# #CommandBasedRobot.__init__()
# # Instances of classes
#
# # Instantiate Subsystems
# # self.shifters = Shifters()
self.drivetrain = Drivetrain(self)
# gc.collect()
# self.shooter = Shooter(self)
# self.carrier = Carrier(self)
# self.feeder = Feeder(self)
# self.intake = Intake(self)
# self.winch = Winch(self)
# self.climber = Climber(self)
#
# # Instantiate Joysticks
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
def __init__(self):
self.left_motor = \
Motor(
self.LEFT_MOTOR_PORT, # port
self.LEFT_MOTOR_REVERSE_POLARITY # switch_polarity
)
self.right_motor = \
Motor(
self.RIGHT_MOTOR_PORT, # port
self.RIGHT_MOTOR_REVERSE_POLARITY # switch_polarity
)
self.drivetrain = \
Drivetrain(
self.left_motor, # left_motor
self.right_motor, # right_motor
self.WHEEL_TRAVEL_MM, # wheel_travel_mm
self.TRACK_MM # track_mm
)
self.controller = Joystick()
self.controller.set_deadband(self.CONTROLLER_DEADBAND)
from vex import (Motor, Touchled, Ports, DistanceUnits, ColorHue)
from vex import (FORWARD, RIGHT, 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 & 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)
# ĐỊNH NGHĨA HẰNG SỐ, THAM SỐ
# ===========================
robot_started = False
drivetrain.set_drive_velocity(100) # dinh nghia toc do mac dinh cua robot
# ĐỊNH NGHĨA CÁC HÀM
# ==================
def flash_led(color, duration_in_seconds):
touch_led.on_hue(color)
sys.sleep(duration_in_seconds)
)
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
)
ARM_MOTOR_VELOCITY_PERCENT = 30
# Claw motor configs
CLAW_MOTOR = Motor(Ports.PORT11, # index
False # reverse
)
CLAW_MOTOR.set_timeout(3, # time
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)
# ĐỊNH NGHĨA HẰNG SỐ, THAM SỐ
# ===========================
robot_started = False
drivetrain.set_drive_velocity(100)
from vex import (Motor, Touchled, Bumper, Ports, DistanceUnits, ColorHue)
from vex import (FORWARD, LEFT, DEGREES)
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 & 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
Motor,
TouchLed,
UNIT_CM
)
# Drive Base configs
LEFT_MOTOR = Motor(1, # port
False # switch_polarity
)
RIGHT_MOTOR = Motor(6, # port
True # switch_polarity
)
DRIVETRAIN = Drivetrain(LEFT_MOTOR, # left_motor
RIGHT_MOTOR, # right_motor
200, # wheel_travel_mm
176 # track_mm
)
# Arm motor configs
ARM_MOTOR = Motor(10, # index
False # switch_polarity
)
ARM_MOTOR_VELOCITY_PERCENT = 30
# Claw motor configs
CLAW_MOTOR = Motor(11, # index
False # switch_polarity
)
CLAW_MOTOR.stall_timeout = 3
CLAW_MOTOR_VELOCITY_PERCENT = 60
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Ố
# ===========================
# mm, khoảng cách robot cần di chuyển để tiến một ô trong mê cung
GRID_SIZE = 308
NORTH = 'N'
EAST = 'E'
class Robot(wpilib.TimedRobot):
def robotInit(self):
print('robotinit')
self.slowMode = True
self.drivetrain = Drivetrain()
self.elevator = Elevator()
self.driver = wpilib.XboxController(JOYSTICK_PORT)
self.intake = Intake()
self.limelight = CitrusLumen()
self.autonTimer = wpilib.Timer()
self.autonCase = None
self.autonAbort = None
self.debugging = False
def robotPeriodic(self):
pass
def autonomousInit(self):
self.drivetrain.resetEncoders()
self.intake.resetEncoders()
self.intake.autonTimerPrep()
self.autonCase = 0
self.autonAbort = False
self.speedMultiplier = 1.0
self.autonTimer.reset()
def autonomousPeriodic(self):
if driver.getStartButtonPressed():
self.autonAbort = True
if self.autonAbort:
self.driverControl()
else:
'''right start'''
if START_POSITION == 1:
if self.autonCase == 0:
if self.drivetrain.autonStraight(3.0):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 1:
self.autonTimer.start()
if self.autonTimer.get() > 5.0:
self.autonDrive(-0.4, 0.4, 1.0, 1.0)
self.autonTimer.stop()
self.autonTimer.reset()
elif self.autonCase == 2:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 3:
if self.drivetrain.autonDrivetrain(0.4, 0.4, 3.0, 3.0):
self.autonTimer.start()
if self.autonTimer.get() > 5.0:
self.drivetrain.resetEncoders()
self.autonCase += 1
self.autonTimer.stop()
self.autonTimer.reset()
'''left start'''
if START_POSITION == -1:
if self.autonCase == 0:
if self.drivetrain.autonPID(2.0):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 1:
if self.drivetrain.autonTurning(1.5):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 2:
if self.drivetrain.autonPID(2.0):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 3:
if self.drivetrain.autonTurning(1.5):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 4:
if self.drivetrain.autonPID(2.0):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 5:
if self.drivetrain.autonTurning(1.5):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 6:
if self.drivetrain.autonPID(2.0):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 7:
if self.drivetrain.autonTurning(1.5):
self.drivetrain.resetEncoders()
self.autonCase += 1
'''center start'''
if START_POSITION == 0:
if CENTER_TRANSPORT_RIGHTLEFT == 1:
if self.autonCase == 0:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 1:
self.autonDrive(-0.4, 0.4, 1.0, 1.0)
elif self.autonCase == 2:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 3:
self.autonDrive(0.4, -0.4, 1.0, 1.0)
elif self.autonCase == 4:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 5: #limelight
if self.drivetrain.autonLimeDrive(
self.limelight.forwardSpeed(),
self.limelight.horizontalHatchSpeed(),
self.limelight.targetArea()):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 6: #raise elevator
if self.elevator.autonElevator(HATCH_BOTTOM):
self.autonCase += 1
elif self.autonCase == 7: #lower tilt to horizontal
if self.intake.autonAngle(160):
self.autonCase += 1
'''center left'''
if CENTER_TRANSPORT_RIGHTLEFT == -1:
if self.autonCase == 0:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 1:
self.autonDrive(0.4, -0.4, 1.0, 1.0)
elif self.autonCase == 2:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 3:
self.autonDrive(-0.4, 0.4, 1.0, 1.0)
elif self.autonCase == 4:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 5: #limelight
if self.drivetrain.autonLimeDrive(
self.limelight.forwardSpeed(),
self.limelight.horizontalHatchSpeed(),
self.limelight.targetArea()):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 6: #raise elevator
if self.elevator.autonElevator(HATCH_BOTTOM):
self.autonCase += 1
elif self.autonCase == 7: #lower tilt to horizontal
if self.intake.autonAngle(160):
self.autonCase += 1
elif START_POSITION == 3:
if self.autonCase == 0:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
elif self.autonCase == 1:
self.autonTimer.start()
if self.autonTimer.get() > 5.0:
self.autonDrive(0.4, 0.4, 3.0, 3.0)
self.autonTimer.stop()
self.autonTimer.reset()
elif self.autonCase == 2:
self.autonTimer.start()
if self.autonTimer.get() > 5.0:
self.autonDrive(-0.4, 0.4, 3.0, 3.0)
self.autonTimer.stop()
self.autonTimer.reset()
elif self.autonCase == 3:
self.autonTimer.start()
if self.autonTimer.get() > 5.0:
self.autonDrive(-0.4, 0.4, 1.0, 1.0)
self.autonTimer.stop()
self.autonTimer.reset()
elif self.autonCase == 4:
self.autonTimer.start()
if self.autonTimer.get() > 5.0:
self.autonDrive(-0.4, 0.4, 1.0, 1.0)
self.autonTimer.stop()
self.autonTimer.reset()
elif self.autonCase == 5: #limelight
if self.drivetrain.autonLimeDrive(
self.limelight.forwardSpeed(),
self.limelight.horizontalHatchSpeed(),
self.limelight.targetArea()):
self.drivetrain.resetEncoders()
self.autonCase += 1
elif self.autonCase == 6: #raise elevator
if self.elevator.autonElevator(HATCH_BOTTOM):
self.autonCase += 1
elif self.autonCase == 7: #lower tilt to horizontal
if self.intake.autonAngle(160):
self.autonCase += 1
def teleopInit(self):
self.drivetrain.resetEncoders()
#set to true for print statements
self.debugging = False
if self.debugging:
print('teleop init')
def teleopPeriodic(self):
self.driverControl()
def autonDrive(self, rV: float, lV: float, rD: float, lD: float) -> bool:
if self.drivetrain.autonDrivetrain(rV, lV, rD, lD):
self.drivetrain.resetEncoders()
self.autonCase += 1
def driverControl(self):
#Variables to hold motor values
driveSpeed, driveAngle = 0, 0
elevatorSpeed = .07 #constant to maintain height
intakeSpeed = 0
intakeAngle = 0
limeArea = 0
usingLime = False
if self.driver.getAButton():
if self.debugging:
print('a')
if self.limelight.targetLocated():
usingLime = True
driveSpeed = self.limelight.forwardSpeed()
driveAngle = self.limelight.horizontalHatchSpeed()
limeArea = self.limelight.targetArea()
if self.debugging:
print(
f'speed: {driveSpeed}, angle: {driveAngle}, area: {limeArea}'
)
else:
if self.debugging:
print('No target found')
elif self.driver.getBButton():
if self.debugging:
print('b')
if self.limelight.targetLocated():
usingLime = True
driveSpeed = self.limelight.forwardSpeed()
driveAngle = self.limelight.horizontalBallSpeed()
limeArea = self.limelight.targetArea()
if self.debugging:
print(
f'speed: {driveSpeed}, angle: {driveAngle}, area: {limeArea}'
)
else:
if self.debugging:
print('No target found')
elif self.driver.getBackButtonPressed():
self.slowMode = not self.slowMode
if self.slowMode:
print('Slow Mode Enabled')
else:
print('Slow Mode Disabled')
if self.debugging:
print('back')
elif self.driver.getStartButtonPressed():
self.limelight.toggleLimelight(False)
if self.debugging:
print('start')
elif self.driver.getStickButtonPressed(0):
if self.debugging:
print('left stick button')
elif self.driver.getStickButtonPressed(1):
if self.debugging:
print('right stick button')
'''intake'''
if self.driver.getBumper(0):
intakeSpeed = -0.5
if self.debugging:
print('left bumper')
elif self.driver.getBumper(1):
intakeSpeed = .8
if self.debugging:
print('right bumper')
if self.driver.getXButton():
intakeAngle = -0.7
if self.debugging:
print('x')
elif self.driver.getYButton():
intakeAngle = 0.7
if self.debugging:
print('y')
'''elevator'''
#left trigger
if self.driver.getTriggerAxis(0) > .05:
elevatorSpeed = -self.driver.getTriggerAxis(
0) * 0.7 #move slower while lowering elevator
if self.debugging:
print(f'left trigger: {self.driver.getTriggerAxis(0)}')
#right trigger
elif self.driver.getTriggerAxis(1) > .05:
elevatorSpeed = self.driver.getTriggerAxis(1)
if self.debugging:
print(f'right trigger: {abs(self.driver.getTriggerAxis(1))}')
'''drivetrain'''
#left and right stick
#up is negative, down is positive
#-------------------------------------
#left stick x axis
if self.driver.getX(0) < -.05 or self.driver.getX(0) > .05:
#driveAngle = self.driver.getX(0)
if self.debugging:
print(f'left x: {self.driver.getX(0)}')
#right stick x axis
if self.driver.getX(1) < -.05 or self.driver.getX(1) > .05:
if self.debugging:
print(f'right x: {self.driver.getX(1)}')
driveAngle = self.driver.getX(1)
#left stick y axis
if self.driver.getY(0) < -.05 or self.driver.getY(0) > .05:
#negative because of the inverted signal
driveSpeed = -self.driver.getY(0)
if self.debugging:
print(f'left y: {-self.driver.getY(0)}')
'''
#this is only for the steering wheel, max values are .71 and -0.81
if driveSpeed > 0:
driveSpeed = self.remap(driveSpeed, 0, .71, 0, 1)
elif driveSpeed < 0:
driveSpeed = self.remap(driveSpeed, 0, -.81, 0, -1)
'''
#right stick y axis
if self.driver.getY(1) < -.05 or self.driver.getY(1) > .05:
#negative because of the inverted signal
if self.debugging:
print(f'right y: {-self.driver.getY(1)}')
if self.slowMode and not usingLime: #lime speed is already reduced
driveAngle = driveAngle * SLOW_SPEED
driveSpeed = driveSpeed * SLOW_SPEED
elevatorSpeed = elevatorSpeed * SLOW_SPEED
if elevatorSpeed < .07 and elevatorSpeed >= 0:
elevatorSpeed = .07
intakeSpeed = intakeSpeed * SLOW_SPEED
intakeAngle = intakeAngle * SLOW_SPEED
#h = self.elevator.getHeight()
#print(h)
if self.debugging:
print(f'drive speed: {driveSpeed}, drive angle {driveAngle} \
elevator speed: {elevatorSpeed}, intake speed: {intakeSpeed} \
intake angle {intakeAngle}')
#apply values to motors
self.drivetrain.arcadeDrive(driveSpeed, driveAngle)
self.elevator.translateElevator(elevatorSpeed)
self.intake.setSpeed(intakeSpeed)
self.intake.setAngle(intakeAngle)
#maps x, which has a range of a-b, to a range of c-d
def remap(self, x, a, b, c, d):
y = (x - a) / (b - a) * (d - c) + c
return y
# ================================
from vex import (Motor, Sonar, Ports, DistanceUnits)
from vex import FORWARD, REVERSE
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)
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 cảm biến khoảng cách
distance_front = Sonar(Ports.PORT7)
# ĐỊNH NGHĨA HẰNG SỐ, THAM SỐ
# ===========================
drivetrain.set_drive_velocity(100)
# Dòng này gây lỗi, bạn có biết nguyên nhân là gì không?
# distance_start = distance_front.distance(DistanceUnits=DistanceUnits.CM)
distance_start = distance_front.distance(DistanceUnits.CM)
print('Ban dau, robot cach tuong so CM la:', distance_start)
# CHƯƠNG TRÌNH CHÍNH
# ==================
#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)
