def main(): imu = imu_sensor.IMU() enc = encoders.Encoders() fast_loop_freq = Config['pygame']['fast_loop_freq'] # 60 Hz loop speed print_lock = threading.Lock() state_vector_lock = threading.Lock() path_planning_lock = threading.Lock() fast_thread = threading.Thread(target=fast_loop, args=(print_lock, state_vector_lock, path_planning_lock, imu, enc, fast_loop_freq), name="fast_loop") fast_thread.start() slow_thread = threading.Thread(target=slow_loop, args=(print_lock, 'dummy_text'), name="slow_loop") slow_thread.start()
def main():
imu = imu_sensor.IMU()
enc = encoders.Encoders()
# lidar = lidar.Lidar() # add when written...
print_lock = threading.Lock()
state_vector_lock = threading.Lock()
path_planning_lock = threading.Lock()
fast_thread = threading.Thread(target=fast_loop,
args=(print_lock, state_vector_lock,
path_planning_lock, imu, enc,
fast_loop_freq),
name="fast_loop")
fast_thread.start()
slow_thread = threading.Thread(target=slow_loop,
args=(print_lock, 'dummy_text'),
name="slow_loop")
slow_thread.start()
date = datetime.datetime.now().strftime("%H-%M-%S-%B-%d-%Y")
path = "sensor_tests/"
motor_file = "motor_ticks_"
camera_file = "camera_coords_"
imu_file = "imu_"
extension = ".csv"
file_num = 1
counter = 1
def get_file_path(sensor_file):
log_file_path = path + sensor_file + date + extension
return log_file_path
enc = encoders.encoder_handler()
IMU = imu_sensor.IMU()
start = time.time()
end = time.time()
dt_time = end - start
while True:
'''
if dt_time >= dt_thresh:
enc.ticks_last = enc.ticks
start = time.time()
enc.return_motor_info() # update ticks
enc.dt_ticks[0] = enc.ticks[0] - enc.ticks_last[0]
enc.dt_ticks[1] = enc.ticks[1] - enc.ticks_last[1]
end = time.time()
from Config import Config
freq = Config['pygame']['fast_loop_freq']
dt = 1 / freq
init = Config['test']['initial_state']
x = np.array(
[[init['x_0'], init['vx_0'], init['y_0'], init['vy_0'],
init['theta_0']]]).T
A = np.array([[1, dt, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, 1, dt, 0],
[0, 0, 0, 1, 0], [0, 0, 0, 0, 1]])
B = np.array([[0.5 * dt**2, 0, 0], [dt, 0, 0], [0, 0.5 * dt**2, 0], [0, dt, 0],
[0, 0, 1]])
imu = imu_sensor.IMU()
clock = Clock()
print("Start Coords - X: {}, Y: {}".format(x[0], x[3]))
input("Enter a key to start loop...")
while True:
a_x = imu.linear_acceleration[0]
a_y = imu.linear_acceleration[1]
theta = imu.euler[0]
u = np.array([[a_x, a_y, theta]]).T
z = np.dot(A, x) + np.dot(B, u)