def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.servo = factory.create_servo() self.sonar = factory.create_sonar() # Add the IP-address of your computer here if you run on the robot self.virtual_create = factory.create_virtual_create() self.map = lab8_map.Map("lab8_map.json") self.odometry = Odometry() self.pidTheta = PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True) # constants self.base_speed = 100 self.variance_sensor = 0.1 self.variance_distance = 0.01 self.variance_direction = 0.05 self.world_width = 3.0 # the x self.world_height = 3.0 # the y self.filter = ParticleFilter(self.virtual_create, self.variance_sensor, self.variance_distance, self.variance_direction, num_particles=100, world_width=self.world_width, world_height=self.world_height)
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ # read file and get all paths self.img = lab11_image.VectorImage("lab11_img1.yaml") # init objects self.create = factory.create_create() self.time = factory.create_time_helper() self.penholder = factory.create_pen_holder() self.tracker = Tracker(factory.create_tracker, 1, sd_x=0.01, sd_y=0.01, sd_theta=0.01, rate=10) self.servo = factory.create_servo() self.odometry = Odometry() # alpha for tracker self.alpha_x = 0.3 self.alpha_y = self.alpha_x self.alpha_theta = 0.3 # init controllers self.pidTheta = PIDController(200, 5, 50, [-10, 10], [-200, 200], is_angle=True) self.pidDistance = PIDController(500, 0, 50, [0, 0], [-200, 200], is_angle=False) self.filter = ComplementaryFilter( self.odometry, None, self.time, 0.4, (self.alpha_x, self.alpha_y, self.alpha_theta)) # parameters self.base_speed = 100 # constant self.robot_marker_distance = 0.1906 # debug vars self.debug_mode = True self.odo = [] self.actual = [] self.xi = 0 self.yi = 0 self.init = True self.penholder_depth = -0.025
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.odometry = Odometry() self.pd_controller = PDController(500, 100, -75, 75) # self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50) self.pid_controller = PIDController(250, 30, 1, -100, 100, -100, 100)
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.odometry = Odometry() self.x_arr = np.array([]) self.y_arr = np.array([]) self.x_arr_truth = np.array([]) self.y_arr_truth = np.array([]) self.time_arr = np.array([])
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.odometry = Odometry( robotProperties["diameter_left"], robotProperties["diameter_right"], robotProperties["wheel_base"], robotProperties["encoder_count"] ) # self.my_robot = MyRobot(None, self.create, self.time, self.odometry) self.prev_r_count = 0 self.prev_l_count = 0
def run(): # the execution of all code shall be started from within this function print('Hello World') planet = Planet() odometry = Odometry() movement_functions = Movement(motor_list = (ev3.LargeMotor('outB'), ev3.LargeMotor('outC')), odometry = odometry) line_follower = follow.LineFollowing(colour_sensor = ev3.ColorSensor('in2'), ts_list = (ev3.TouchSensor('in1'), ev3.TouchSensor('in4')), movement=movement_functions, odometry = odometry) communication = Communication(planet = planet, odometry = odometry) communication.connect() line_follower.colour_calibration() line_follower.line_following() communication.first_communication() sleep(2) line_follower.path_recognising() while True: #odometry.odometry_calculations() line_follower.line_following() line_follower.path_recognising()
def __init__(self, mqtt_client, logger): # Create Planet and planet_name variable self.planet = Planet() self.planet_name = None # Setup Sensors, Motors and Odometry self.rm = ev3.LargeMotor("outB") self.lm = ev3.LargeMotor("outC") self.sound = ev3.Sound() self.odometry = Odometry(self.lm, self.rm) self.motors = Motors(self.odometry) self.cs = ColorSensor(self.motors) self.us = Ultrasonic() # Setup Communication self.communication = Communication(mqtt_client, logger, self) # Create variable to write to from communication self.start_location = None self.end_location = None self.path_choice = None self.running = True
def __init__(self, device_path): self.device = serial.Serial(device_path, 115200, timeout=0.5) self.write_lock = threading.Lock() self.device.write(chr(128) * 3) self.device.write(chr(131)) self.odometry = Odometry() self.bumpers = [False, False] self.drops = [False, False] self.wall = False self.cliff = [False, False, False, False] self.vwall = False self.dirt = 0 self.buttons = [False, False, False] self.charging = 0 self.charge_current = 0 self.charge_voltage = 0 self._odom_left = 0 self._odom_right = 0 self._odom_last = [0, 0] self._odom_start = True self.joint_positions = [0, 0, 0, 0] self.temperature = 0 self._packets = [ 7, 8, 9, 10, 11, 12, 13, 14, 15, 18, 43, 44, 21, 22, 24, 25 ] self._i = 0 self._unmeasured = [False, False]
def guide(sh_mem, setting, log_datetime): print('Im Guide') def shutdown_child(signum=None, frame=None): try: sleep(0.2) log_file = open("./log/log_{}_guide.csv".format(log_datetime), 'w') for log in logs: if log != "": log_file.write("{}\n".format(log)) log_file.close() if ('line_tracer' in globals()) or ('line_tracer' in locals()): line_tracer.shutdown() sys.exit() except Exception as ex: g_log.exception(ex) raise ex def wait_for_input(): print('Guide Waiting ...') sh_mem.write_guide_is_ready_mem(1) while not sh_mem.read_touch_sensor_mem(): sleep(0.1) signal.signal(signal.SIGTERM, shutdown_child) try: # ここで変数定義などの事前準備を行う # Time of each loop, measured in miliseconds. loop_time_millisec = 18 # Time of each loop, measured in seconds. loop_time_sec = loop_time_millisec / 1000.0 # ログ記録用 logs = ["" for _ in range(10000)] log_pointer = 0 wait_for_input() # 設置が終わるまで待つ line_tracer = LineTracer(setting) # プログラム実行時にキャリブレーションを実行する場合は以下のコードを実行する # print('Calibrate ColorSensor ...') # line_tracer.calibrate_color_sensor() print('Configurating Odometry ...') odometry = Odometry() print('Guide is Ready') # タッチセンサー押し待ち wait_for_input() speed_reference = 0 direction = 0 odometry_speed_reference = 0 odometry_direction = 0 refrection_raw = 0 angle_l = 0 angle_r = 0 # スタート時の時間取得 t_line_trace_start = clock() while True: ############################################################### ## Loop info ############################################################### t_loop_start = clock() # ここでライントレースする speed_reference, direction, refrection_raw = line_tracer.line_tracing( ) # 角度を算出してオドメトリーを使用 angle_l = sh_mem.read_motor_encoder_left_mem() angle_r = sh_mem.read_motor_encoder_right_mem() # odometry_speed_reference, odometry_direction = odometry.target_trace(angle_l,angle_r) # direction = odometry_direction # speed_reference = odometry_speed_reference # 左右モーターの角度は下記のように取得 # print(read_motor_encoder_left_mem()) # print(read_motor_encoder_right_mem()) # 前進後退・旋回スピードは下記のように入力 sh_mem.write_speed_mem(speed_reference) sh_mem.write_steering_mem(int(round(direction))) # 実行時間、PID制御に関わる値をログに出力 t_loop_end = clock() # logs[log_pointer] = "{}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}".format( logs[log_pointer] = "{}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}".format( t_loop_end - t_line_trace_start, t_loop_end - t_loop_start, speed_reference, direction, refrection_raw, line_tracer.refrection_target, line_tracer.e_b, line_tracer.p_b, line_tracer.i_b, line_tracer.d_b, angle_l, angle_r # , # odometry.pre_pos_x, # odometry.pre_pos_y, # odometry_direction, # odometry_speed_reference ) log_pointer += 1 ############################################################### ## Busy wait for the loop to complete ############################################################### sleep(max(loop_time_sec - (clock() - t_loop_start), 0.002)) except Exception as e: print("It's a Guide Exception") g_log.exception(e) shutdown_child()
from odometry import Odometry from transforms import * import sensors import time import math print("CCW is the positive direction!") angle = float(input("Input the desired turn angle-> ")) dt = 0.01 odometry = Odometry(dt) sensors.initSensors() heading_setpoint = Rotation2d.fromDegrees(0) # left A Right B def setHeading(offset): global heading_setpoint sensors.updateSensors() deg = odometry.getFieldToVehicle().getRotation().getDegrees() heading_setpoint = Rotation2d.fromDegrees(deg).rotateBy(Rotation2d.fromDegrees(offset)) print("Heading set to: " + str(heading_setpoint)) def getHeadingError(): current_heading = odometry.getFieldToVehicle().getRotation() return current_heading.inverse().rotateBy(heading_setpoint).getDegrees() turn_speed = 0.15 sensors.setLeftMotor(-turn_speed * angle / math.fabs(angle)) sensors.setRightMotor(turn_speed * angle / math.fabs(angle))
class Robot(object): cycle_time = 0.01 enabled = False odometry = Odometry(cycle_time) def __init__(self, gui): self.gui = gui self.drive = Drive(self, gui) def initialize(self): sensors.initSensors() def startLoop(self): if (not self.enabled): self.gui.log_message("Starting Main Robot Loop") self.enabled = True self.initialize() self.drive.initDrive() self.odometry.reset() self.drive.setEnableIntersection(self.gui.getIntersectionEnabled()) self.drive.setEnableEntering(self.gui.getEnteringEnabled()) self.main_thread = threading.Thread(target=self.loop) self.main_thread.start() def stopLoop(self): if (self.enabled): self.gui.log_message("Stopping Main Robot Loop") self.enabled = False def loop(self): loop_counter = 0 self.current = RigidTransform2d(Translation2d(0, 0), Rotation2d.fromDegrees(0)) while (self.enabled): loop_counter += 1 #self.gui.log_message("robot main") # Able to decrease sensor update frequency if (loop_counter % 1 == 0): sensors.updateSensors() self.odometry.updateOdometry() self.drive.updateDrive() if (loop_counter % 1 == 0): self.current = self.odometry.getFieldToVehicle() self.gui.log_pos(self.current) self.gui.log_sonics(sensors.getLeftWallDistance(), sensors.getForwardWallDistance(), sensors.getRightWallDistance()) self.gui.log_mag(sensors.getMagneticMagnitude()) self.gui.log_ir(0.0, 0.0) self.gui.log_state(self.drive.state) if (loop_counter >= 1000): loop_counter = 0 time.sleep(self.cycle_time) sensors.shutdown()
from odometry import Odometry from controller import WheeledRobotController from math import pi from ev3dev import ev3 from logger import Logger odometry = Odometry(ev3.LargeMotor("outB"), ev3.LargeMotor("outC"), 6.0, 2.7, 10.0, 2 * pi / 5.0) controller = WheeledRobotController(ev3.UltrasonicSensor("in2"), ev3.UltrasonicSensor("in1"), ev3.UltrasonicSensor("in3"), odometry) logger = Logger("resources/map.csv") controller.run_closed_loop(kp=0.025, ki=0.0, kd=0.012, avg_n=4, stop_threshold=5.0, logger=logger, delta=0.1) logger.complete()
def __init__(self): self.odometry = Odometry() self.BASE_SLEEP_TIME_US = 0.250 * 1000000
if not bad_data: predictions.append([i, j, k]) errors.append(math.log(error)) if len(errors) > 0: ix = errors.index(min(errors)) return predictions[ix] return -1 if __name__ == '__main__': init() step() odometry = Odometry(ENCODER_UNIT * (positionLeft.getValue()), ENCODER_UNIT * (positionRight.getValue()), INIT_X, INIT_Y, INIT_ANGLE) count = 0 while (True): odometry_info = odometry.track_step( ENCODER_UNIT * (positionLeft.getValue()), ENCODER_UNIT * (positionRight.getValue())) if not step(): # print('saving data') data_collector.collect(x_odometry, y_odometry, theta_odometry, x, y, theta, np.array(distance_sensors_info)) plot()
from frame_loader import FrameLoader from odometry import FeatureExtractor from odometry import Odometry from visualizator import Visualizator # Declarations frame_loader = FrameLoader("data/video.mp4") extractor = FeatureExtractor("orb") visual_odometry = Odometry() visualizator = Visualizator() frame_read = True while frame_read: # Load and display the frame frame_read, frame = frame_loader.get_frame() # Extract features and descriptors kp, des = extractor.extract_features(frame) # Visualize results visualizator.show_image(frame) visualizator.show_keypoints(frame, kp) # Printouts print("Captured frame: %d" % frame_loader.frame_num) print("Frame shape: %s" % (frame.shape,)) print("Extracted keypoints: %d" % len(kp)) frame_loader.close()