class Motors(object):
def __init__(self, a_star, odometer):
self.a_star = a_star
self.odometer = odometer
self.command = None
self.left_motor = Motor()
self.right_motor = Motor()
self.a_star.motors(0, 0)
self.odometer.add_measurement_callback(self._odom_measurement_callback)
def set_speed_target(self, left, right):
if not self.command or not isinstance(self.command, Speed):
self.command = Speed(left, right)
else:
self.command.set_target_speed(left, right)
if left == 0 and right == 0:
self.odometer.stop_tracking()
else:
self.odometer.track_odometry()
def move_forward(self, distance, speed):
self.command = MoveStraight(
self.odometer.get_situation().dist + distance, speed,
self.odometer.get_situation().yaw,
self.odometer.get_situation().dist)
self.odometer.track_odometry()
def rotate(self, angle, speed):
self.command = Rotate(
(self.odometer.get_situation().yaw + angle) % (2 * pi), speed)
self.odometer.track_odometry()
def stop(self):
self.set_speed_target(0, 0)
def _odom_measurement_callback(self, situation):
if self.command and not self.command.is_achieved():
speed_left, speed_right = self.command.get_motor_speeds(situation)
if self.command.is_achieved():
self.odometer.stop_tracking()
else:
speed_left, speed_right = 0, 0
self.left_motor.set_speed(speed_left)
self.right_motor.set_speed(speed_right)
left_cmd = self.left_motor.get_speed_command(situation.speed_l,
situation.current_time)
right_cmd = self.right_motor.get_speed_command(situation.speed_r,
situation.current_time)
self._send_command_to_motors(left_cmd, right_cmd)
def _send_command_to_motors(self, left, right):
self.a_star.motors(left, right)
def main():
servo = Servo()
#servo.test()
ctrl = Controller(servo)
cmd = [\
Rotate(horizontal=-90, deg=True),\
Rotate(horizontal=90, deg=True),\
Default(),\
Rotate(vertical=-90, deg=True),\
Rotate(vertical=90, deg=True),\
Default()\
]
ctrl.execute_commands(cmd)
def to_servo(self, x, y):
"""角度表記を信号幅に変換する
Args:
x (int): Android端末スクリーン上のx座標
y (int): Android端末スクリーン上のy座標
"""
cmd = Rotate(horizontal=x, vertical=y)
self.ctrl.execute(cmd)
print(cmd)
def set_speed_target(self, left, right):
if not self.command or not isinstance(self.command, Speed):
self.command = Speed(left, right)
else:
self.command.set_target_speed(left, right)
if left == 0 and right == 0:
self.odometer.stop_tracking()
else:
self.odometer.track_odometry()
def _rotate(self, direction, degrees):
'''Rotates the robot in the direction specified by the given degrees.
params direction: Direction in which to start moving. ie Robot.LEFT is counter-clockwise'''
assert isinstance(direction, int)
if not (direction == Robot.LEFT or direction == Robot.RIGHT):
raise RuntimeError("Turning direction is not recognized.")
if degrees < 0 or degrees > 360:
raise RuntimeError("Invalid degrees.")
if degrees == 0:
return
self._addCommand(Rotate(direction, degrees))
def main():
servo = Servo()
ctrl = Controller(servo)
#縦方向の動作テストコマンド
vertical_test_cmd = [\
Default(),\
Rotate(vertical=SERVO_MIN),\
Rotate(vertical=SERVO_MAX),\
Default(),\
Rotate(vertical=SERVO_MIN),\
Rotate(vertical=SERVO_MAX),\
Default()\
]
#横方向の動作テストコマンド
horizontal_test_cmd = [\
Default(),\
Rotate(horizontal=SERVO_MIN),\
Rotate(horizontal=SERVO_MAX),\
Default(),\
Rotate(horizontal=SERVO_MIN),\
Rotate(horizontal=SERVO_MAX),\
Default()\
]
#縦と横の両方向を組み合わせた動作テストコマンド
combine_test_cmd = [\
Default(),\
Rotate(vertical=SERVO_MIN, horizontal=SERVO_MIN),\
Rotate(vertical=SERVO_MAX, horizontal=SERVO_MAX),\
Default(),\
Rotate(vertical=SERVO_MIN, horizontal=SERVO_MIN),\
Rotate(vertical=SERVO_MAX, horizontal=SERVO_MAX),\
Default()\
]
#実行
ctrl.execute_commands(vertical_test_cmd)
ctrl.execute_commands(horizontal_test_cmd)
ctrl.execute_commands(combine_test_cmd)
def rotate(self, angle, speed):
self.command = Rotate(
(self.odometer.get_situation().yaw + angle) % (2 * pi), speed)
self.odometer.track_odometry()
def move_forward(self, distance, speed):
self.command = MoveStraight(
self.odometer.get_situation().dist + distance, speed,
self.odometer.get_situation().yaw,
self.odometer.get_situation().dist)
self.odometer.track_odometry()