class Robot:
def __init__(self):
self.sound = Sound()
self.direction_motor = MediumMotor(OUTPUT_D)
self.swing_motorL = LargeMotor(OUTPUT_A)
self.swing_motorC = LargeMotor(OUTPUT_B)
self.swing_motorR = LargeMotor(OUTPUT_C)
self.swing_motors = [
self.swing_motorL, self.swing_motorC, self.swing_motorR
]
self.touch_sensor = TouchSensor(INPUT_1)
self.console = Console(DEFAULT_FONT)
self.buttons = Button()
self.beeps_enabled = True
def beep(self, frequency=700, wait_for_comeplete=False):
if not self.beeps_enabled:
return
play_type = Sound.PLAY_WAIT_FOR_COMPLETE if wait_for_comeplete else Sound.PLAY_NO_WAIT_FOR_COMPLETE
self.sound.beep("-f %i" % frequency, play_type=play_type)
def calibrate_dir(self):
''' Calibrate direction motor '''
# Run motor with 25% power, and wait until it stops running
self.direction_motor.on(SpeedPercent(-10), block=False)
# while (not self.direction_motor.STATE_OVERLOADED):
# print(self.direction_motor.duty_cycle)
self.direction_motor.wait_until(self.direction_motor.STATE_OVERLOADED)
self.direction_motor.stop_action = Motor.STOP_ACTION_COAST
self.direction_motor.stop()
time.sleep(.5)
# Reset to straight
# self.direction_motor.on_for_seconds(SpeedPercent(10), .835, brake=True, block=True)
self.direction_motor.on_for_degrees(SpeedPercent(10),
127,
brake=True,
block=True)
self.direction_motor.reset()
print("Motor reset, position: " + str(self.direction_motor.position))
time.sleep(.5)
def calibrate_swing(self):
for m in self.swing_motors:
m.stop_action = m.STOP_ACTION_HOLD
m.on(SpeedPercent(6))
self.swing_motorC.wait_until(self.swing_motorC.STATE_OVERLOADED, 2000)
for m in self.swing_motors:
m.stop_action = m.STOP_ACTION_HOLD
m.on_for_degrees(SpeedPercent(5), -15, brake=True, block=False)
self.swing_motorC.wait_while('running')
for m in self.swing_motors:
m.reset()
m.stop_action = m.STOP_ACTION_HOLD
m.stop()
print("Ready Angle: %i" % self.swing_motorC.position)
def ready_swing(self, angle: int):
right_angle = -(angle / 3)
# adjust motors to target angle
for m in self.swing_motors:
m.stop_action = Motor.STOP_ACTION_HOLD
m.on_for_degrees(SpeedPercent(2),
right_angle,
brake=True,
block=False)
self.swing_motorC.wait_while('running')
for m in self.swing_motors:
m.stop_action = Motor.STOP_ACTION_HOLD
m.stop()
print("Swing Angle: %i" % self.swing_motorC.position)
def set_direction(self, direction):
print("Setting direction to: " + str(direction))
#direction = self.__aim_correction(direction)
self.direction_motor.on_for_degrees(SpeedPercent(10),
round(direction * 3))
print("Direction set to: " + str(self.direction_motor.position))
#
# def __aim_correction(self, direction):
# x = direction
# y = -0.00000000429085685725*x**6 + 0.00000004144345630728*x**5 + 0.00001219331494759860*x**4 + 0.00020702946527870400*x**3 + 0.00716486965517779000*x**2 + 1.29675836037884000000*x + 0.27064829453014400000
#
# return y
def shoot(self, power):
print("SHOOT, power: %i" % power)
for m in self.swing_motors:
m.duty_cycle_sp = -power
for m in self.swing_motors:
m.run_direct()
time.sleep(.5)
self.swing_motorC.wait_until_not_moving()
for m in self.swing_motors:
m.reset()
def wait_for_button(self):
self.touch_sensor.wait_for_bump()
def __set_display(self, str):
self.console.set_font(font=LARGER_FONT, reset_console=True)
self.console.text_at(str, column=1, row=1, reset_console=True)
def wait_for_power_select(self, power=0, angle=0, steps=1):
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
def left():
power -= steps
if power < 0:
power = 0
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['left'], timeout_ms=150)
def right():
power += steps
if power > 100:
power = 100
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['right'], timeout_ms=150)
def up():
angle += steps
if angle > 110:
angle = 110
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['up'], timeout_ms=150)
def down():
angle -= steps
if angle < 0:
angle = 0
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['down'], timeout_ms=150)
while not self.touch_sensor.is_pressed:
if self.buttons.left:
left()
elif self.buttons.right:
right()
elif self.buttons.up:
up()
elif self.buttons.down:
down()
self.console.set_font(font=DEFAULT_FONT, reset_console=True)
return (power, angle)
def select_connection_mode(self):
self.__set_display("Enable Connection\nLeft: True - Right: False")
enabled = True
while not self.touch_sensor.is_pressed:
if self.buttons.left:
enabled = True
self.buttons.wait_for_released(buttons=['left'])
break
elif self.buttons.right:
enabled = False
self.buttons.wait_for_released(buttons=['right'])
break
self.console.set_font(font=DEFAULT_FONT, reset_console=True)
return enabled
def print(self, string):
print(string)
