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