def __init__(self, file_name, directory, map_name, map_dir, FilterClass, **plot_info): # project.set_project_dir("rccar") self.checkpoints = get_map("checkpoints.txt") self.waypoints = Waypoints( map_name, map_dir=map_dir ) initial_long, initial_lat = self.checkpoints[0] second_long, second_lat = self.checkpoints[1] initial_heading = FilterClass.get_gps_bearing( second_long, second_lat, initial_long, initial_lat ) filter = FilterClass( standard_params['counts_per_rotation'], standard_params['wheel_radius'], standard_params['front_back_dist'], standard_params['max_speed'], standard_params['left_angle_limit'], standard_params['right_angle_limit'], standard_params['left_servo_limit'], standard_params['right_servo_limit'], initial_long, initial_lat, initial_heading ) super(FilterTest, self).__init__( file_name, directory, filter, plot_info )
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