blob: 157c4a5ef46f2128b741ba14b3b3f1480b4f77a3 [file] [log] [blame]
Niko Sohmers11017112024-02-18 16:03:58 -08001#!/usr/bin/python3
2
3from aos.util.trapezoid_profile import TrapezoidProfile
4from frc971.control_loops.python import control_loop
5from frc971.control_loops.python import angular_system
6from frc971.control_loops.python import controls
7import numpy
8import sys
9from matplotlib import pylab
10import gflags
11import glog
12
13FLAGS = gflags.FLAGS
14
15try:
16 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
17except gflags.DuplicateFlagError:
18 pass
19
20gflags.DEFINE_bool('hybrid', False, 'If true, make it hybrid.')
21
22kAltitude = angular_system.AngularSystemParams(
23 name='Altitude',
24 motor=control_loop.KrakenFOC(),
25 G=(16.0 / 60.0) * (16.0 / 162.0),
26 # 4340 in^ lb
27 J=1.27,
28 q_pos=0.40,
29 q_vel=3.0,
30 kalman_q_pos=0.12,
31 kalman_q_vel=2.0,
32 kalman_q_voltage=4.0,
33 kalman_r_position=0.05,
34 radius=10.5 * 0.0254)
35
36
37def main(argv):
38 if FLAGS.plot:
39 R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
40 angular_system.PlotKick(kAltitude, R)
41 angular_system.PlotMotion(kAltitude, R)
42 return
43
44 # Write the generated constants out to a file.
45 if len(argv) != 7:
46 glog.fatal(
47 'Expected .h file name and .cc file name for the intake and integral intake.'
48 )
49 else:
50 namespaces = ['y2024', 'control_loops', 'superstructure', 'altitude']
51 angular_system.WriteAngularSystem(kAltitude, argv[1:4], argv[4:7],
52 namespaces)
53
54
55if __name__ == '__main__':
56 argv = FLAGS(sys.argv)
57 glog.init()
58 sys.exit(main(argv))