-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain_snn_pid.py
84 lines (60 loc) · 3.02 KB
/
main_snn_pid.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
import setup_path
import airsim
from gains import PIDGains, VelocityControllerGains
from simulator_drone import *
import time
import numpy as np
from snn_pid import model, probes, p_ensemble, integrator, d_ensemble, radius, gains, n_neurons
from snn_pid_controller import SnnPidController, scale_factor
from visualization import plot_results, visualize_tuning_curves_basis_functions, visualize_stimulus_response
from decorators import timer
num_points = 200
setpoints = np.ones(num_points) * 30
actuals = np.zeros(num_points)
print(f"num_points.size: {num_points}")
@timer
def run_simulation(drone, pid_controller):
last_error = 0
for sample_idx, sp in enumerate(setpoints):
drone.log("SP: ", f"{sp}")
actuals[sample_idx] = drone.get_height()
err = sp - actuals[sample_idx]
drone.log("ACTUAL: ", f"{actuals[sample_idx]}")
drone.log("ERROR: ", f"{err}")
if abs(actuals[sample_idx] - sp) < 2:
print(f"-------------------\nreach {sp} at {sample_idx}-------------------\n")
u, p, i, d = pid_controller.get_pid(err, last_error)
last_error = err
print(f"[{sample_idx}] height: {actuals[sample_idx]}, err:{err}, P:{p}, I:{i}, D:{d}, PID: {u}")
drone.rc_command(u)
@timer
def run_main():
drone = AirSimDrone(AirSimDrone.snn_u_to_velocity)
drone.log("BEGIN: ", f"before lift off..")
# adjust AirSim inner-loop PID
pidGains = VelocityControllerGains(z_gains=PIDGains(10, 10, 10))
drone._client.setVelocityControllerGains(pidGains)
pid_controller = SnnPidController() # IMPL-NOTE: Gains defines in snn_pid.py for NengoGUI
height = drone.lift_off()
drone.log("BEGIN: ", f"start height {height}")
time.sleep(3)
run_simulation(drone, pid_controller)
drone.close()
print(f"setpoints.size: {setpoints.size}, actuals.size: {actuals.size}")
plot_results(setpoints, actuals,
suptitle="SNN-PID",
title=f"{gains} [AirSim\\Nengo period: {configurations.AIRSIM_SIMULATION_MS_PERIOD}, {configurations.NENGO_SIMULATION_MS_PERIOD}; scale: {scale_factor}; radius: {radius}; neurons: {n_neurons}]")
#post_run_render(pid_controller)
def post_run_render(pid_controller):
visualize_stimulus_response(pid_controller._sim, probes["stimulus"], probes["spikes_P"], probes["voltage_P"])
visualize_stimulus_response(pid_controller._sim, probes["stimulus"], probes["spikes_I"], probes["voltage_I"])
visualize_stimulus_response(pid_controller._sim, probes["stimulus"], probes["spikes_D"], probes["voltage_D"])
def pre_run_render():
#visualize Tuning Curves, Basis Functions
visualize_tuning_curves_basis_functions(model, p_ensemble, title="P-term ensemble")
visualize_tuning_curves_basis_functions(model, integrator.ensemble, title="I-term ensemble")
visualize_tuning_curves_basis_functions(model, d_ensemble, title="D-term ensemble")
pass
if __name__ == '__main__':
#pre_run_render()
run_main()