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