def predict_state_test():
input("Enter key when in position: ")
enc = sensors.Encoders()
imu = sensors.IMU(
) # reinstantiate each time to avoid errors due to drift in IMU
l, r = enc.get_ticks() # clears arduino in case there was prior movement
theta_0 = get_heading(
imu
) # should = 0 (I think...) It might initialise it to the Earth's Magnetic field but I can't remember. Either way it's fine...
x = np.array(
[[init['x_0'], init['vx_0'], init['y_0'], init['vy_0'], theta_0]]).T
num_steps = 0
print("Ready to start!")
clock = Clock()
while True:
try:
l, r = enc.get_ticks()
x = oss.measure_state(x, l, r)
num_steps += 1
clock.tick(freq)
except KeyboardInterrupt:
break
print()
theta_t = get_heading(imu) # get true final heading
x_t = int(input("Enter true x position (mm): "))
y_t = int(input("Enter true y position (mm): "))
# data = [x_pred, y_pred, theta_pred, x_t, y_t, theta_t, num_steps, freq]
data = [x[0][0], x[1][0], x[4][0], x_t, y_t, theta_t, num_steps, freq]
log_data(data)
print()
print(data)
print()
def main():
# Sensor init
imu = sensors.IMU()
enc = sensors.Encoders()
lidar = sensors.Lidar()
# State Space Models
rss = models.RobotStateSpace()
iss = models.IMUStateSpace()
oss = models.OdometryStateSpace()
lss = models.LidarStateSpace()
# Control init
ctrl = control.Control()
# Path Planning init
#extents = input("Enter world extents: e.g. 3000 2200")
#start = input("Enter start pose: e.g. 500 200 0").split()
#goal = input("Enter goal pose: e.g. 2800 0 0")
extents = Config['path_planning']['test_params']['extents']
start = Config['path_planning']['test_params']['start']
goal = Config['path_planning']['test_params']['goal']
# Obstacle Detection
obstacles_lidar = []
heading = (-1 * np.radians(imu.euler[0] - iss.theta_bias)) % (2 * np.pi)
scan = lidar.get_scan(100,
0.04) # add more updates to improve initial scan
obstacles_lidar = lss.get_obstacles(scan, heading)
print(obstacles_lidar)
# Scaling down
extents = (int(extents[0] / d), int(extents[1] / d))
start = (start[0] / d, (start[1] / d) + (extents[1] / 2),
np.radians(start[2]))
goal = (goal[0] / d, (goal[1] / d) + (extents[1] / 2), np.radians(goal[2]))
obstacles = []
for point in obstacles_lidar:
obstacles.append(
(int(point[0] / 20 + start[0]), (int(point[1] / 20 + start[1]))))
pp = kastar.KinematicAStar(extents)
path_scaled_down, moves, visited, obstacles_array = pp.plan_path(
start, goal, obstacles)
# scaling path coords back to real scale
path = []
for a, b in path_scaled_down:
log_data(path_file, [a * d, b * d])
path.append((a * d, b * d))
print(path)
print(moves)
input("Ready? ")
print(3)
time.sleep(1)
print(2)
time.sleep(1)
print(1)
time.sleep(1)
print("GO!")
# Threading init
print_lock = threading.Lock()
state_vector_lock = threading.Lock()
fast_thread = threading.Thread(target=fast_loop,
args=(print_lock, state_vector_lock, imu,
enc, iss, oss, rss, path, ctrl),
name="fast_loop")
fast_thread.start()
import sensors
import csv
import time
save_name = input("Name of save file: ")
file_path = "/home/pi/BEng_Project/sensor_tests/data/imu/{}.csv".format(save_name)
freq = 60
dt = 1 / freq
counter = 0
time_elapsed = 0
imu = sensors.IMU()
time.sleep(2) # to make sure I don't disturb the data!
start_time = time.time()
while True:
lin_acc_x = imu.linear_acceleration[0]
lin_acc_y = imu.linear_acceleration[1]
lin_acc_z = imu.linear_acceleration[2]
theta_a = imu.euler[0]
theta_b = imu.euler[1]
theta_c = imu.euler[2]
counter += 1
with open(file_path, 'a', newline='') as file:
writer = csv.writer(file)
writer.writerow([lin_acc_x, lin_acc_y, lin_acc_z, theta_a, theta_b, theta_c])