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)