Plot currents in the AngularSystem plots
These are most useful for trying to understand power usage during
typical movements, both at the rotor when trying to determine motor
heating, and at the battery when looking at breaker sizing.
Change-Id: I6616a11cfbaf02cf177ed73bf34fc80817ab39c1
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 7ccb4af..cad3221 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -193,6 +193,8 @@
x_plot = []
v_plot = []
a_plot = []
+ motor_current_plot = []
+ battery_current_plot = []
x_goal_plot = []
v_goal_plot = []
x_hat_plot = []
@@ -239,7 +241,14 @@
v_goal_plot.append(end_goal[1, 0])
U = U_uncapped.copy()
+
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+
+ motor_current = (U[0, 0] - plant.X[1, 0] / plant.G / plant.motor.Kv
+ ) / plant.motor.resistance
+ motor_current_plot.append(motor_current)
+ battery_current = U[0, 0] * motor_current / 12.0
+ battery_current_plot.append(battery_current)
x_plot.append(plant.X[0, 0])
if v_plot:
@@ -282,8 +291,16 @@
pylab.plot(t_plot, offset_plot, label='voltage_offset')
pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(t_plot, a_plot, label='a')
+ ax1 = pylab.subplot(3, 1, 3)
+ ax1.set_xlabel("time(s)")
+ ax1.set_ylabel("rad/s^2")
+ ax1.plot(t_plot, a_plot, label='a')
+
+ ax2 = ax1.twinx()
+ ax2.set_xlabel("time(s)")
+ ax2.set_ylabel("Amps")
+ ax2.plot(t_plot, battery_current_plot, 'g', label='battery')
+ ax2.plot(t_plot, motor_current_plot, 'r', label='motor')
pylab.legend()
pylab.show()