| justinT21 | 942892b | 2024-07-02 22:33:50 -0700 | [diff] [blame] | 1 | #!/usr/bin/python3 |
| 2 | |
| 3 | import numpy |
| 4 | import math |
| 5 | import scipy.integrate |
| 6 | |
| 7 | from matplotlib import pylab |
| 8 | import sys |
| 9 | import gflags |
| 10 | import glog |
| 11 | |
| Austin Schuh | 0f88109 | 2024-06-28 15:36:48 -0700 | [diff] [blame] | 12 | from frc971.control_loops.swerve.numpy_dynamics import swerve_physics |
| justinT21 | 942892b | 2024-07-02 22:33:50 -0700 | [diff] [blame] | 13 | |
| 14 | FLAGS = gflags.FLAGS |
| 15 | |
| 16 | |
| 17 | def u_func(X): |
| 18 | result = numpy.zeros([8, 1]) |
| 19 | # result[1, 0] = 80.0 |
| 20 | # result[3, 0] = 80.0 |
| 21 | # result[5, 0] = -80.0 |
| 22 | # result[7, 0] = -80.0 |
| 23 | return result |
| 24 | |
| 25 | |
| 26 | def main(argv): |
| 27 | x_initial = numpy.zeros([25, 1]) |
| 28 | # x_initial[0] = -math.pi / 2.0 |
| 29 | x_initial[3] = 1.0 / (2.0 * 0.0254) |
| 30 | # x_initial[4] = math.pi / 2.0 |
| 31 | x_initial[7] = 1.0 / (2.0 * 0.0254) |
| 32 | # x_initial[8] = -math.pi / 2.0 |
| 33 | x_initial[11] = 1.0 / (2.0 * 0.0254) |
| 34 | # x_initial[12] = math.pi / 2.0 |
| 35 | x_initial[15] = 1.0 / (2.0 * 0.0254) |
| 36 | x_initial[19] = 2.0 |
| 37 | result = scipy.integrate.solve_ivp(swerve_physics, (0, 2.0), |
| 38 | x_initial.reshape(25, ), |
| 39 | max_step=0.01, |
| 40 | args=(u_func, )) |
| 41 | |
| 42 | cm = pylab.get_cmap('gist_rainbow') |
| 43 | fig = pylab.figure() |
| 44 | ax = fig.add_subplot(111) |
| 45 | ax.set_prop_cycle(color=[cm(1. * i / 25) for i in range(25)]) |
| 46 | ax.plot(result.t, result.y[0, :], label="thetas0", linewidth=7.0) |
| 47 | ax.plot(result.t, result.y[1, :], label="thetad0", linewidth=7.0) |
| 48 | ax.plot(result.t, result.y[2, :], label="omegas0", linewidth=7.0) |
| 49 | ax.plot(result.t, result.y[3, :], label="omegad0", linewidth=7.0) |
| 50 | ax.plot(result.t, result.y[4, :], label="thetas1", linewidth=7.0) |
| 51 | ax.plot(result.t, result.y[5, :], label="thetad1", linewidth=7.0) |
| 52 | ax.plot(result.t, result.y[6, :], label="omegas1", linewidth=7.0) |
| 53 | ax.plot(result.t, result.y[7, :], label="omegad1", linewidth=7.0) |
| 54 | ax.plot(result.t, result.y[8, :], label="thetas2", linewidth=7.0) |
| 55 | ax.plot(result.t, result.y[9, :], label="thetad2", linewidth=7.0) |
| 56 | ax.plot(result.t, result.y[10, :], label="omegas2", linewidth=7.0) |
| 57 | ax.plot(result.t, result.y[11, :], label="omegad2", linewidth=7.0) |
| 58 | ax.plot(result.t, result.y[12, :], label="thetas3", linewidth=7.0) |
| 59 | ax.plot(result.t, result.y[13, :], label="thetad3", linewidth=7.0) |
| 60 | ax.plot(result.t, result.y[14, :], label="omegas3", linewidth=7.0) |
| 61 | ax.plot(result.t, result.y[15, :], label="omegad3", linewidth=7.0) |
| 62 | ax.plot(result.t, result.y[16, :], label="x", linewidth=7.0) |
| 63 | ax.plot(result.t, result.y[17, :], label="y", linewidth=7.0) |
| 64 | ax.plot(result.t, result.y[18, :], label="theta", linewidth=7.0) |
| 65 | ax.plot(result.t, result.y[19, :], label="vx", linewidth=7.0) |
| 66 | ax.plot(result.t, result.y[20, :], label="vy", linewidth=7.0) |
| 67 | ax.plot(result.t, result.y[21, :], label="omega", linewidth=7.0) |
| 68 | ax.plot(result.t, result.y[22, :], label="Fx", linewidth=7.0) |
| 69 | ax.plot(result.t, result.y[23, :], label="Fy", linewidth=7.0) |
| 70 | ax.plot(result.t, result.y[24, :], label="Moment", linewidth=7.0) |
| 71 | numpy.set_printoptions(threshold=numpy.inf) |
| 72 | print(result.t) |
| 73 | print(result.y) |
| 74 | pylab.legend() |
| 75 | pylab.show() |
| 76 | |
| 77 | return 0 |
| 78 | |
| 79 | |
| 80 | if __name__ == '__main__': |
| 81 | argv = FLAGS(sys.argv) |
| 82 | glog.init() |
| 83 | sys.exit(main(argv)) |