Add climber code without tests
This runs a loop to follow the request.
Signed-off-by: Siddhant Kanwar <kanwarsiddhant@gmail.com>
Change-Id: Iad516390561175f74eb33e0cda6a505376a372ed
diff --git a/frc971/constants.h b/frc971/constants.h
index 7f1f1d5..6188f01 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -68,6 +68,8 @@
double allowable_encoder_error;
};
+struct RelativeEncoderZeroingConstants {};
+
struct AbsoluteEncoderZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index 7ca61b7..049b52f 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -237,39 +237,42 @@
}
table RelativeEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool (id: 0);
+
// The state of the subsystem, if applicable. -1 otherwise.
// TODO(alex): replace with enum.
- state:int (id: 0);
+ state:int (id: 1);
// If true, we have aborted.
- estopped:bool (id: 1);
+ estopped:bool (id: 2);
// Position of the joint.
- position:float (id: 2);
+ position:float (id: 3);
// Velocity of the joint in units/second.
- velocity:float (id: 3);
+ velocity:float (id: 4);
// Profiled goal position of the joint.
- goal_position:float (id: 4);
+ goal_position:float (id: 5);
// Profiled goal velocity of the joint in units/second.
- goal_velocity:float (id: 5);
+ goal_velocity:float (id: 6);
// Unprofiled goal position from absoulte zero of the joint.
- unprofiled_goal_position:float (id: 6);
+ unprofiled_goal_position:float (id: 7);
// Unprofiled goal velocity of the joint in units/second.
- unprofiled_goal_velocity:float (id: 7);
+ unprofiled_goal_velocity:float (id: 8);
// The estimated voltage error.
- voltage_error:float (id: 8);
+ voltage_error:float (id: 9);
// The calculated velocity with delta x/delta t
- calculated_velocity:float (id: 9);
+ calculated_velocity:float (id: 10);
// Components of the control loop output
- position_power:float (id: 10);
- velocity_power:float (id: 11);
- feedforwards_power:float (id: 12);
+ position_power:float (id: 11);
+ velocity_power:float (id: 12);
+ feedforwards_power:float (id: 13);
// State of the estimator.
- estimator_state:frc971.RelativeEncoderEstimatorState (id: 13);
+ estimator_state:frc971.RelativeEncoderEstimatorState (id: 14);
}
table StaticZeroingSingleDOFProfiledSubsystemGoal {
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index c1bfe8c..ea47580 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -104,6 +104,9 @@
ESTOP,
};
+ bool zeroed() const { return profiled_subsystem_.zeroed(); }
+ bool estopped() const { return state() == State::ESTOP; }
+
State state() const { return state_; }
private:
@@ -264,7 +267,7 @@
.template BuildStatus<typename ProfiledJointStatus::Builder>(
status_fbb);
- status_builder.add_estopped(state_ == State::ESTOP);
+ status_builder.add_estopped(estopped());
status_builder.add_state(static_cast<int32_t>(state_));
return status_builder.Finish();
}
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 5ae7ffd..59434a8 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -187,7 +187,7 @@
position->index_pulses = encoder.index_posedge_count();
}
- // Copies a relative encoder.
+ // Copies a relative digital encoder.
void CopyPosition(const ::frc::Encoder &encoder,
::frc971::RelativePositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
@@ -199,6 +199,16 @@
encoder_ratio);
}
+ // Copies a potentiometer
+ void CopyPosition(const ::frc::AnalogInput &input,
+ ::frc971::RelativePositionT *position,
+ ::std::function<double(double)> potentiometer_translate,
+ bool reverse, double pot_offset) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * potentiometer_translate(input.GetVoltage()) + pot_offset;
+ }
+
double encoder_translate(int32_t value, double counts_per_revolution,
double ratio) {
return static_cast<double>(value) / counts_per_revolution * ratio *
diff --git a/frc971/zeroing/relative_encoder_test.cc b/frc971/zeroing/relative_encoder_test.cc
index fd86fb3..a29ba4b 100644
--- a/frc971/zeroing/relative_encoder_test.cc
+++ b/frc971/zeroing/relative_encoder_test.cc
@@ -1,8 +1,6 @@
#include "frc971/zeroing/zeroing.h"
-
-#include "gtest/gtest.h"
-
#include "frc971/zeroing/zeroing_test.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
@@ -21,7 +19,8 @@
TEST_F(RelativeEncoderZeroingTest, TestRelativeEncoderZeroingWithoutMovement) {
PositionSensorSimulator sim(1.0);
- RelativeEncoderZeroingEstimator estimator;
+ RelativeEncoderZeroingEstimator estimator{
+ constants::RelativeEncoderZeroingConstants{}};
sim.InitializeRelativeEncoder();
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 0e90e3f..028194c 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -126,10 +126,12 @@
// A trivial ZeroingEstimator which just passes the position straight through.
class RelativeEncoderZeroingEstimator
- : public ZeroingEstimator<RelativePosition, void,
+ : public ZeroingEstimator<RelativePosition,
+ constants::RelativeEncoderZeroingConstants,
RelativeEncoderEstimatorState> {
public:
- explicit RelativeEncoderZeroingEstimator() {}
+ explicit RelativeEncoderZeroingEstimator(
+ const constants::RelativeEncoderZeroingConstants &) {}
// Update position with new position from encoder
void UpdateEstimate(const RelativePosition &position) override {