Add relative encoder zeroing strategy
Change-Id: I07b5461a9766aa25c9c858b7771af26ed08ad4ff
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index 4f1e888..a95d407 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -61,6 +61,14 @@
absolute_encoder:double;
}
+// Represents all of the data for a single encoder.
+// The relative encoder values are relative to where the encoder was at some
+// arbitrary point in time.
+table RelativePosition {
+ // Current position read from the encoder.
+ encoder:double;
+}
+
// The internal state of a zeroing estimator.
table EstimatorState {
// If true, there has been a fatal error for the estimator.
@@ -105,6 +113,14 @@
absolute_position:double;
}
+table RelativeEncoderEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ error:bool;
+
+ // The estimated position of the joint.
+ position:double;
+}
+
// The internal state of a zeroing estimator.
table IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 612801e..b874b1d 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -91,6 +91,10 @@
negedge_value_ = start_position;
}
+void PositionSensorSimulator::InitializeRelativeEncoder() {
+ current_position_ = 0.0;
+}
+
void PositionSensorSimulator::MoveTo(double new_position) {
{
const int lower_start_segment = lower_index_edge_.current_index_segment();
@@ -218,5 +222,14 @@
return builder->Finish();
}
+template <>
+flatbuffers::Offset<RelativePosition>
+PositionSensorSimulator::GetSensorValues<RelativePositionBuilder>(
+ RelativePositionBuilder *builder) {
+ builder->add_encoder(current_position_);
+ return builder->Finish();
+}
+
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 6be6bbc..faa5dd5 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,10 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
-#include "flatbuffers/flatbuffers.h"
-
#include "aos/testing/random_seed.h"
-
+#include "flatbuffers/flatbuffers.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/gaussian_noise.h"
@@ -50,6 +48,11 @@
double known_index_lower,
double known_index_upper);
+ // Initializes a sensor simulation with a single relative enoder, providing
+ // position information relative to a starting position of 0. For use with
+ // RelativePosition sensor values.
+ void InitializeRelativeEncoder();
+
// Simulate the structure moving to a new position. The new value is measured
// relative to absolute zero. This will update the simulated sensors with new
// readings.
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index 46bc9fc..e4b2f25 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -197,6 +197,42 @@
estimator_state:frc971.AbsoluteEncoderEstimatorState;
}
+table RelativeEncoderProfiledJointStatus {
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.RelativeEncoderEstimatorState;
+}
+
table StaticZeroingSingleDOFProfiledSubsystemGoal {
unsafe_goal:double;