def get_action(self, sensors=None):
self._set_sensors(sensors)
if self._state == State.WAITING_TO_START:
# Flat, do nothing
if abs(self._pitch) < 0.2 and abs(self._roll) < 0.2:
return None
# Not flat, use roll to set direction of turn.
# We're going to always head down hill if tilted
if self._roll < -0.2:
self._turn_right = False
elif self._roll > 0.2:
self._turn_right = True
return MotionCommand(-0.2, 0)
elif self._state == State.REVERSING:
return MotionCommand(-0.2, 0)
elif self._state == State.TURNING and self._turn_right:
return MotionCommand(0, -1.5)
elif self._state == State.TURNING and not self._turn_right:
return MotionCommand(0, 1.5)
return None
def get_action(self, sensors=None):
self._set_sensors(sensors)
if self._state == State.WAITING_TO_START:
if not self._is_sensor_active():
return None
return MotionCommand(-0.2, 0)
elif self._state == State.REVERSING:
return MotionCommand(-0.2, 0)
elif self._state == State.TURNING:
return MotionCommand(0, 1.5)
return None
def __init__(self, wheel_separation, max_speed):
"""
:param wheel_separation: in metres
:param max_speed: in m/sec
"""
self._wheel_separation = wheel_separation
self._max_motor_speed = max_speed
self._motion_command = MotionCommand()
def get_action(self, sensors=None):
self._set_sensors(sensors)
if self._state == State.WAITING_TO_START:
if not self._is_sensor_active():
return None
if self._fl_set:
self._turn_right = True
elif self._fr_set:
self._turn_right = False
return MotionCommand(-0.2, 0)
elif self._state == State.REVERSING:
return MotionCommand(-0.2, 0)
elif self._state == State.TURNING and self._turn_right:
return MotionCommand(0, -1.5)
elif self._state == State.TURNING and not self._turn_right:
return MotionCommand(0, 1.5)
return None
def __init__(self, screen):
super().__init__()
self._run = False
self._screen = screen
self._command = MotionCommand()
self._lock = Lock()
a_star = AStar()
base = RomiRobotBase(wheel_separation, max_speed, a_star)
left_bumper = SwitchSensor(name='left_bumper', pin=27)
right_bumper = SwitchSensor(name='right_bumper', pin=22)
distance = UltrasonicDistanceSensor(name='distance', echo=19, trigger=18)
encoders = RomiEncoderSensor('encoders', a_star)
pose_sensor = RomiPoseSensor('pose', base=base, encoders=encoders)
imu_sensor = RomiImuSensor('imu')
sensors = Sensors(sensors=[
pose_sensor, encoders, left_bumper, right_bumper, distance, imu_sensor
])
cruise_command = MotionCommand(0.2, 0.0)
cruise_behaviour = Cruise(0, cruise_command)
#teleop_keys_behaviour = TeleopKeys(0)
bump_escape_behaviour = RomiBumpEscape(3)
usonic_escape_behaviour = RomiUsonicEscape(2)
imu_escape_behaviour = RomiImuEscape(1)
#behaviours = [teleop_keys_behaviour, escape_behaviour]
behaviours = [
cruise_behaviour, usonic_escape_behaviour, bump_escape_behaviour,
imu_escape_behaviour
]
arbitrator = Arbitrator(behaviours)
server = Server(host='192.168.1.101', port=65432)
server.start()
def __init__(self, priority, waypoints):
super(GoToWaypoints, self).__init__(priority, "go_to_waypoints")
self._waypoints = waypoints
self._command = MotionCommand()
self._state = State.WAITING_TO_START
def __init__(self, wheel_separation, max_speed):
super().__init__(wheel_separation, max_speed)
self._motion_command = MotionCommand()
self._count = 0
def get_value(self):
motion = self._base.get_motion_command()
self._count = self._count + 1
if motion.rotation == 0.0:
self._dist_left = self._dist_left + motion.velocity
self._dist_right = self._dist_left
elif motion.velocity == 0.0:
self._dist_left = self._dist_left - motion.rotation
self._dist_right = self._dist_right + motion.rotation
return [self._dist_left, self._dist_right]
wheel_separation = 0.133 # metres
max_speed = 0.61
mock_base = MockBase(wheel_separation, max_speed)
mock_base.set_motion_command(MotionCommand(velocity=1.0, rotation=0.0))
pose_sensor = RomiPoseSensor(name='pose',
pose={
'x': 0.0,
'y': 0.0,
'theta': 0.0
},
base=mock_base,
encoders=MockEncoders(base=mock_base))
for i in range(0, 10):
print(pose_sensor.get_value())
mock_base.set_motion_command(MotionCommand(velocity=0.0, rotation=0.01))
for i in range(0, 10):
print(pose_sensor.get_value())