Exemplo n.º 1
0
    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
Exemplo n.º 3
0
    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)
Exemplo n.º 4
0
    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([])
Exemplo n.º 5
0
    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
Exemplo n.º 6
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()
Exemplo n.º 7
0
    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
Exemplo n.º 8
0
    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]
Exemplo n.º 9
0
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()
Exemplo n.º 10
0
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))
Exemplo n.º 11
0
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()
Exemplo n.º 12
0
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()
Exemplo n.º 13
0
 def __init__(self):
     self.odometry = Odometry()
     self.BASE_SLEEP_TIME_US = 0.250 * 1000000
Exemplo n.º 14
0
                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()
Exemplo n.º 15
0
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()