def reset_odometry(self): self.reading_count = 0 self.dynamics = Dynamics() self.velocity = 0.0 self.last_velocity = 0.0 self.heading_adjustment = 0. self.odometer_start = 0 self.odometer_front_left_start = 0 self.odometer_front_right_start = 0 self.odometer_back_left_start = 0 self.odometer_back_right_start = 0 self.ackerman = Ackerman( front_wheelbase_width = self.front_wheelbase_width_in_meters, wheelbase_length = self.wheelbase_length_in_meters)
class Car: def __init__(self, online = True): self.config_path = 'car.ini' self.read_configuration() self.online = online self.reset_odometry() self.listener = None self.last_verified_velocity = 0.0 self.lcd = None self.usb_error_count = 0 self.input_recording_file = None self.commands_recording_file = None if self.online: from lcd import Lcd self.lcd = Lcd() self.quit = False self.write_command('td+') # first command normally fails, so write a blank command self.write_command('td+') self.output_thread = threading.Thread(target=self._monitor_output, args = ()) self.output_thread.daemon = True self.output_thread.start() while self.dynamics.reading_count == 0: time.sleep(0.01) def display_text(self, s): if self.lcd is not None: self.lcd.display_text(s) # records all input from the pi def begin_recording_input(self, recording_file_path): if self.input_recording_file is not None: self.end_recording_input() self.input_recording_file = open(recording_file_path,'w') def end_recording_input(self): if self.input_recording_file is not None: self.input_recording_file.close() self.input_recording_file = None # records all commands to the pi def begin_recording_commands(self, commands_recording_file_path): if self.commands_recording_file is not None: self.end_recording_commands() self.commands_recording_file = open(commands_recording_file_path,'w') def end_recording_commands(self): if self.commands_recording_file is not None: self.commands_recording_file.close() self.commands_recording_file = None def reset_odometry(self): self.reading_count = 0 self.dynamics = Dynamics() self.velocity = 0.0 self.last_velocity = 0.0 self.heading_adjustment = 0. self.odometer_start = 0 self.odometer_front_left_start = 0 self.odometer_front_right_start = 0 self.odometer_back_left_start = 0 self.odometer_back_right_start = 0 self.ackerman = Ackerman( front_wheelbase_width = self.front_wheelbase_width_in_meters, wheelbase_length = self.wheelbase_length_in_meters) def read_configuration(self): # odometry self.meters_per_odometer_tick = float(self.get_option('calibration','meters_per_odometer_tick')) self.gyro_adjustment_factor = float(self.get_option('calibration','gyro_adjustment_factor')) # esc and steering self.center_steering_us = int(self.get_option('calibration','center_steering_us')) self.min_forward_esc = int(self.get_option('calibration','min_forward_esc')) self.min_reverse_esc = int(self.get_option('calibration','min_reverse_esc')) self.reverse_center_steering_us = int(self.get_option('calibration','reverse_center_steering_us')) # car dimensions self.front_wheelbase_width_in_meters = float(self.get_option('calibration','front_wheelbase_width_in_meters')) self.rear_wheelbase_width_in_meters = float(self.get_option('calibration','rear_wheelbase_width_in_meters')) self.wheelbase_length_in_meters = float(self.get_option('calibration','wheelbase_length_in_meters')) # infer general dimensions # todo: put these in config to get complete shape of car self.length = self.wheelbase_length_in_meters self.width = self.front_wheelbase_width_in_meters def add_listener(self, listener): if self.listener is not None: raise Exception("only one listener allowed") self.listener = listener def remove_listener(self, listener): if self.listener is not listener: raise Exception("Unknown listener") self.listener = None def __exit__(self, type_unused, value_unused, traceback_unused): self.quit = True if self.online: self.output_thread.join() def get_option(self,section,option): config = ConfigParser.ConfigParser() with open(self.config_path, 'rwb+') as configfile: config.readfp(configfile) return config.get(section, option) def set_option(self,section,option,value): config = ConfigParser.ConfigParser() with open(self.config_path, 'rwb+') as configfile: config.readfp(configfile) config.set(section,option,value) with open(self.config_path, 'rwb+') as configfile: config.write(configfile) def write_command(self, s): if not self.online: raise Exception("must be online") l = "{0}\n".format(s) with open('/dev/car','w') as command: command.write(l) if self.commands_recording_file is not None: self.commands_recording_file.write(l) def _monitor_output(self): self.output = open('/var/log/car','r') self.output.seek(0,2) # go to end of file line_start = None while not self.quit: s = self.output.readline() if s: if line_start is not None: s = line_start + s line_start = None if not s.endswith('\n'): line_start = s continue self.process_line_from_log(s) else: time.sleep(0.001) def process_line_from_log(self, s): # sanity check. Only process valid TRACE_DYNAMICS log lines fields = s.split(',') if fields == None: return if len(fields) < 10: return if fields[1] != 'TD': return if len(fields) != 39: self.usb_error_count = self.usb_error_count + 1 #print 'invalid TD packet with {} fields: {}'.format(len(fields),s) return if self.input_recording_file is not None: self.input_recording_file.write(s) # todo, handle contention with different threads # for now, make changeover quick and don't mess # with existing structures, maybe by keeping a big list current = Dynamics() try: current.set_from_log(fields) except: return previous = self.dynamics self.dynamics = current self.reading_count += 1 if self.reading_count > 1: self.apply_dynamics(current, previous) else: self.original_dynamics = current if self.listener != None: self.listener.put(current) def get_usb_error_count(self): return self.usb_error_count # returns ping distance in meters def ping_inches(self): return self.ping_meters()/0.0254 def ping_distance(self): return self.ping_meters() def ping_meters(self): return self.dynamics.ping_millimeters / 1000. def battery_voltage(self): return self.dynamics.battery_voltage def apply_dynamics(self, current, previous): # correct heading with adjustment factor self.heading_adjustment += (1. - self.gyro_adjustment_factor) * standardized_degrees(current.heading - previous.heading) # if wheels have moved, update ackerman wheel_distance_meters = (current.odometer_ticks-previous.odometer_ticks) * self.meters_per_odometer_tick if abs(wheel_distance_meters) > 0.: outside_wheel_angle = radians(self.angle_for_steering(previous.str)) self.ackerman.move_left_wheel(outside_wheel_angle, wheel_distance_meters, self.heading_radians()) # update velocity if current.odometer_last_us != previous.odometer_last_us: elapsed_seconds = (current.odometer_last_us - previous.odometer_last_us) / 1000000. self.velocity = wheel_distance_meters / elapsed_seconds self.last_verified_velocity = self.velocity else: # no tick this time, how long has it been? seconds_since_tick = ( current.us - current.odometer_last_us ) / 1000000. if seconds_since_tick > 0.1: # it's been a long time, let's call velocity zero self.velocity = 0.0 else: # we've had a tick recently, fastest possible speed is when a tick is about to happen # let's use smaller of that and previously certain velocity max_possible = self.meters_per_odometer_tick / seconds_since_tick if max_possible > abs(self.last_verified_velocity): self.velocity = self.last_verified_velocity else: if self.last_verified_velocity > 0: self.velocity = max_possible else: self.velocity = -max_possible #print("x:{:.2f} y:{:.2f} heading:{:.2f}".format(self.ackerman.x, self.ackerman.y, relative_heading)) # returns the velocity in meters/sec # if the car is reversing, velocity will be negative def get_velocity_meters_per_second(self): return self.velocity; # returns position of rear of car (between two rear wheels), this starts at -wheelbase_length_in_meters,0 def rear_position(self): return (self.ackerman.x - self.wheelbase_length_in_meters, self.ackerman.y) # returns position of front of car (between two front wheels), this starts at 0,0 def front_position(self): h = radians(self.heading_degrees()); l = self.wheelbase_length_in_meters return (self.ackerman.x + l * cos(h) - l , self.ackerman.y + l * sin(h)) def heading_degrees(self): return standardized_degrees(self.dynamics.heading - self.original_dynamics.heading + self.heading_adjustment) def heading_radians(self): return radians(self.heading_degrees()) def set_rc_mode(self): self.write_command('rc') def set_manual_mode(self): self.write_command('m') def wheels_angle(self): return self.angle_for_steering(self.dynamics.str) def esc_for_velocity(self, v): data = [ (-2., 1200), # made up numbers for negative v: todo: find empirically (-1., 1250), (0.0, 1500), (0.1, 1645), (0.34, 1659), (0.85, 1679), (1.2, 1699), (1.71, 1719), (1.88, 1739), (2.22, 1759), (2.6, 1779), (3.0, 1799), (14.0, 2000) ] return table_lookup(data, v) def angle_for_steering(self, str): data = [ (30,1000), (25,1104), (20,1189), (15,1235), (10,1268), (5, 1390), (0, 1450), (-5, 1528), (-10, 1607), (-15,1688), (-20, 1723), (-25, 1768), (-30, 1858)] last = len(data)-1 if str <= data[0][1]: return data[0][0] if str >= data[last][1]: return data[last][0] for i in range(0,last): if str <= data[i+1][1]: return interpolate( str, data[i][1], data[i][0], data[i+1][1], data[i+1][0]) # returns the steering pulse for give steering angle # of the outside wheel def steering_for_angle(self, theta): data = [ (30,1000), (25,1104), (20,1189), (15,1235), (10,1268), (5, 1390), (0, 1450), (-5, 1528), (-10, 1607), (-15,1688), (-20, 1723), (-25, 1768), (-30, 1858)] last = len(data)-1 if theta >= data[0][0]: return data[0][1] if theta <= data[last][0]: return data[last][1] for i in range(0,last): if theta >= data[i+1][0]: return interpolate( theta, data[i][0], data[i][1], data[i+1][0], data[i+1][1]) def set_esc_and_str(self, speed, steering): self.write_command('pse {0},{1}'.format(int(steering), int(speed))) def zero_odometer(self): self.odometer_start = self.dynamics.odometer_ticks self.odometer_front_left_start = self.dynamics.odometer_front_left self.odometer_front_right_start = self.dynamics.odometer_front_right self.odometer_back_left_start = self.dynamics.odometer_back_left self.odometer_back_right_start = self.dynamics.odometer_back_right def odometer_meters(self): return (self.dynamics.odometer_ticks - self.odometer_start) * self.meters_per_odometer_tick def odometer_front_left(self): return self.dynamics.odometer_front_left - self.odometer_front_left_start def odometer_front_right(self): return self.dynamics.odometer_front_right - self.odometer_front_right_start def odometer_back_left(self): return self.dynamics.odometer_back_left - self.odometer_back_left_start def odometer_back_right(self): return self.dynamics.odometer_back_right - self.odometer_back_right_start # returns where you should steer to if you wish to go to goal_heading def steering_for_goal_heading_degrees(self, goal_heading, reverse = False): direction = -1 if reverse else 1 heading_error = degrees_diff(goal_heading, self.heading_degrees()) steering = self.steering_for_angle(-direction * heading_error) return steering def is_drifting(self): return self.ackerman.is_drifting() def forward(self, meters, goal_heading = None, fixed_steering_us = None, max_speed = 2.0): ticks = int(meters/self.meters_per_odometer_tick) if fixed_steering_us != None: steering = fixed_steering_us if goal_heading == None: goal_heading = self.dynamics.heading #use a direction of 1 / -1 to help with the math for forward / reverse if ticks > 0: direction = 1 min_esc = 1500 max_esc = self.esc_for_velocity(max_speed) else: direction = -1 min_esc = 1500 max_esc = self.esc_for_velocity(-max_speed) goal_odometer = self.dynamics.odometer_ticks + ticks self.set_rc_mode() while self.dynamics.odometer_ticks * direction < goal_odometer * direction: esc = max_esc # adjust steering if fixed steering wasn't selected if fixed_steering_us == None: heading_error = degrees_diff(goal_heading, self.dynamics.heading) steering = self.steering_for_angle(-direction * heading_error) # adjust speed if abs(self.get_velocity_meters_per_second()) > max_speed: esc = min_esc self.set_esc_and_str(esc, steering) time.sleep(.02) self.set_esc_and_str(1500,steering) slowdown_start = time.time() while (time.time()-slowdown_start < 3): if fixed_steering_us == None: heading_error = degrees_diff(goal_heading, self.dynamics.heading) steering = self.steering_for_angle(-direction * heading_error * 1.5) time.sleep(0.01)