Merge changes Ie821842f,Ie8a999cc,I4a07dc92
* changes:
Split UpdateY from Update in StateFeedbackLoop
Decouple profiled subsystem axis from inputs and outputs
Add -m flag to scale signals when plotting
diff --git a/frc971/analysis/logreader.py b/frc971/analysis/logreader.py
index c59487b..c3a789c 100644
--- a/frc971/analysis/logreader.py
+++ b/frc971/analysis/logreader.py
@@ -51,7 +51,7 @@
self.HandleLine(line)
except Exception as ex:
# It's common for the last line of the file to be malformed.
- print("Ignoring malformed log entry: ", line)
+ print("Ignoring malformed log entry: ", line, ex)
def HandleLine(self, line):
"""
@@ -70,13 +70,19 @@
binary = key[0]
struct_instance_name = key[1]
data_search_path = key[2]
- boolean_multiplier = None
+ boolean_multiplier = False
+ multiplier = 1.0
# If the plot definition line ends with a "-b X" where X is a number then
# that number gets drawn when the value is True. Zero gets drawn when the
# value is False.
if len(data_search_path) >= 2 and data_search_path[-2] == '-b':
- boolean_multiplier = float(data_search_path[-1])
+ multiplier = float(data_search_path[-1])
+ boolean_multiplier = True
+ data_search_path = data_search_path[:-2]
+
+ if len(data_search_path) >= 2 and data_search_path[-2] == '-m':
+ multiplier = float(data_search_path[-1])
data_search_path = data_search_path[:-2]
# Make sure that we're looking at the right binary structure instance.
@@ -88,10 +94,10 @@
for path in data_search_path:
data = data[path]
- if boolean_multiplier is not None:
+ if boolean_multiplier:
if data == 'T':
- value.Add(pline.time, boolean_multiplier)
+ value.Add(pline.time, multiplier)
else:
value.Add(pline.time, 0)
else:
- value.Add(pline.time, data)
+ value.Add(pline.time, float(data) * multiplier)
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 4d09bbc..6ebba72 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -24,12 +24,15 @@
// styles.
template <int number_of_states, int number_of_axes,
class ZeroingEstimator =
- ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator,
+ int number_of_inputs = number_of_axes,
+ int number_of_outputs = number_of_axes>
class ProfiledSubsystem {
public:
ProfiledSubsystem(
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>> loop,
+ number_of_states, number_of_inputs, number_of_outputs>>
+ loop,
::std::array<ZeroingEstimator, number_of_axes> &&estimators)
: loop_(::std::move(loop)), estimators_(::std::move(estimators)) {
zeroed_.fill(false);
@@ -55,7 +58,7 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes> &
+ const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs> &
controller() const {
return *loop_;
}
@@ -102,8 +105,8 @@
}
// Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(::std::array<double, number_of_axes> voltages) {
- for (int i = 0; i < number_of_axes; ++i) {
+ void set_max_voltage(::std::array<double, number_of_inputs> voltages) {
+ for (int i = 0; i < number_of_inputs; ++i) {
loop_->set_max_voltage(i, voltages[i]);
}
}
@@ -114,7 +117,8 @@
// TODO(austin): It's a bold assumption to assume that we will have the same
// number of sensors as axes. So far, that's been fine.
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>> loop_;
+ number_of_states, number_of_inputs, number_of_outputs>>
+ loop_;
// The goal that the profile tries to reach.
Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 6d05444..891aaa6 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -160,6 +160,11 @@
// that the plant should deal with.
CheckU(U);
X_ = Update(X(), U);
+ UpdateY(U);
+ }
+
+ // Computes the new Y given the control input.
+ void UpdateY(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
Y_ = C() * X() + D() * U;
}