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())