Exemplo n.º 1
0
    def __init__(self, checkpoints_name=None, log_data=True):
        # set the project name (so that maps and logs and be found)
        project.set_project_dir("roboquasar")

        self.manual_mode = True

        self.goal_x, self.goal_y = 0, 0
        self.checkpoints = get_map(checkpoints_name)
        self.checkpoint_num = 0

        joystick = WiiUJoystick(
            button_down_fn=lambda button: self.button_dn(button),
            # axis_active_fn=lambda axis, value: self.axis_active(axis, value),
             axis_inactive_fn=lambda axis: self.axis_inactive(axis),
            dpad_active_fn=lambda direction: self.dpad(direction),
        )

        sensors = dict(
            gps=dict(sensor_id=1,
                     properties=['lat', 'long', 'altitude', 'geoid_height',
                                 'pdop', 'hdop', 'vdop', 'fix'],
                     update_fn=lambda: self.gps_updated()),
            imu=dict(sensor_id=2, properties=[
                'yaw', 'ax', 'ay', 'az', 'gx', 'gy', 'gz', 'mx', 'my', 'mz'],
                     update_fn=lambda: self.imu_updated()),
        )

        commands = dict(
            leds=dict(command_array={
                "red": 0,
                "yellow": 1,
                "green": 2,
            },
                range=(0, 2),
                mapping={
                    "off": 0,
                    "on": 1,
                    "toggle": 2
                }),
            blue_led=dict(command_id=3, range=(0, 255)),
            stepper=dict(command_id=4, range=(-0x8000, 0x7fff)),
        )

        super(RoboQuasarBot, self).__init__(
            sensors, commands, '/dev/ttyUSB0', joystick=joystick,
            close_fn=self.close_fn, log_data=log_data, log_dir=":today"
        )

        self.imu = self.sensors['imu']
        self.gps = self.sensors['gps']
        
        self.leds = self.commands['leds']
        self.blue_led = self.commands['blue_led']

        self.stepper = self.commands['stepper']

        self.prev_time = time.time()
Exemplo n.º 2
0
    def __init__(self, file_name, directory, **plot_info):
        project.set_project_dir("roboquasar")
        self.checkpoints = get_map("buggy course checkpoints.gpx")
        self.course_map = get_map("buggy course map.gpx")

        super(FilterTest, self).__init__(
            file_name, directory, 1, plot_info
        )
        
        first_gps = self.parser.get(5, "gps")[-1]
        second_gps = self.parser.get(50, "gps")[-1]
        
        lat1, long1, alt1 = first_gps["lat"], first_gps["long"], first_gps["altitude"] 
        lat2, long2, alt2 = second_gps["lat"], second_gps["long"], second_gps["altitude"]
        
        print(lat1, long1, alt1)
        print(lat2, long2, alt2)
        
        initial_yaw, initial_pitch, initial_roll = \
            get_gps_orientation(lat1, long1, alt1,
                                lat2, long2, alt2)
        
        print(np.array([initial_yaw, initial_pitch, initial_roll]) * 180 / np.pi)
        self.filter = GrovesKalmanFilter(
            initial_roll_ecef=initial_roll,
            initial_pitch_ecef=initial_pitch,
            initial_yaw_ecef=initial_yaw,
            initial_lat=lat1,
            initial_long=long1,
            initial_alt=alt1,
            **constants)

        first_gps_reading = self.parser.get(0, 'gps')[-1]
        initial_lat = first_gps_reading["lat"]
        initial_long = first_gps_reading["long"]
        self.draw_starting_dot(initial_long, initial_lat)

        self.prev_lat, self.prev_long = initial_lat, initial_long

        self.prev_imu_t = 0
        self.prev_gps_t = 0
Exemplo n.º 3
0
    def __init__(self, log_data):
        project.set_project_dir("lidar_turret")

        sensors = dict(
            lidar=dict(sensor_id=1,
                     properties=['counts', 'rotations', 'distance'],
                     update_fn=lambda: self.lidar_updated()),
        )

        commands = dict(
            lidar_commands=dict(command_array={
                "resume": 0,
                "pause": 1,
                "direction": 2,
                "speed": 3,
            },
                range=(0, 255),
                mapping={
                    "forward": 0,
                    "backward": 1,
                    "min_speed": 1,
                    "max_speed": 255,
                }
            )
        )

        super(LidarBot, self).__init__(
            sensors, commands, '/dev/tty.usbserial',
            close_fn=self.close_fn, log_data=log_data, log_dir=":today"
        )

        self.lidar = self.sensors["lidar"]
        self.lidar_resume = self.commands["lidar_commands"]["resume"]
        self.lidar_pause = self.commands["lidar_commands"]["pause"]
        self.lidar_direction = self.commands["lidar_commands"]["direction"]
        self.lidar_speed = self.commands["lidar_commands"]["speed"]