Add intake and turret code plus superstructure tests
Signed-off-by: Milo Lin <100027790@mvla.net>
Change-Id: I9885bd1e839ba0356147606415ae915cd295faf6
Change-Id: I33bc83673645869e255136198c0789f722c881a0
Signed-off-by: Siddhartha Chatterjee <ninja.siddhartha@gmail.com>
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
Signed-off-by: Henry Speiser <henry@speiser.net>
diff --git a/y2022/constants.cc b/y2022/constants.cc
index e71fb64..b89ef5a 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -11,8 +11,9 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
-#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
+#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
+#include "y2022/control_loops/superstructure/turret/integral_turret_plant.h"
namespace y2022 {
namespace constants {
@@ -25,42 +26,64 @@
const uint16_t kPracticeTeamNumber = 9971;
const uint16_t kCodingRobotTeamNumber = 7971;
-const Values *DoGetValuesForTeam(uint16_t team) {
- Values *const r = new Values();
+} // namespace
+
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+
+ Values r;
// TODO(Yash): Set constants
// Intake constants.
- auto *const intake = &r->intake;
+ auto *const intake_front = &r.intake_front;
+ auto *const intake_back = &r.intake_back;
- intake->zeroing_voltage = 3.0;
- intake->operating_voltage = 12.0;
- intake->zeroing_profile_params = {0.5, 3.0};
- intake->default_profile_params = {6.0, 30.0};
- intake->range = Values::kIntakeRange();
- intake->make_integral_loop =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ intake_params;
+
+ intake_params.zeroing_voltage = 3.0;
+ intake_params.operating_voltage = 12.0;
+ intake_params.zeroing_profile_params = {0.5, 3.0};
+ intake_params.default_profile_params = {6.0, 30.0};
+ intake_params.range = Values::kIntakeRange();
+ intake_params.make_integral_loop =
control_loops::superstructure::intake::MakeIntegralIntakeLoop;
-
- // The number of samples in the moving average filter.
- intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
- // The distance that the absolute encoder needs to complete a full rotation.
- intake->zeroing_constants.one_revolution_distance =
+ intake_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ intake_params.zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake_params.zeroing_constants.zeroing_threshold = 0.0005;
+ intake_params.zeroing_constants.moving_buffer_size = 20;
+ intake_params.zeroing_constants.allowable_encoder_error = 0.9;
+ intake_params.zeroing_constants.measured_absolute_position = 0.0;
- // Threshold for deciding if we are moving. moving_buffer_size samples need to
- // be within this distance of each other before we use the middle one to zero.
- intake->zeroing_constants.zeroing_threshold = 0.0005;
- // Buffer size for deciding if we are moving.
- intake->zeroing_constants.moving_buffer_size = 20;
+ intake_front->subsystem_params = intake_params;
+ intake_back->subsystem_params = intake_params;
- // Value between 0 and 1 indicating what fraction of one_revolution_distance
- // it is acceptable for the offset to move.
- intake->zeroing_constants.allowable_encoder_error = 0.9;
+ // TODO(Yash): Set constants
+ // Turret constants.
+ auto *const turret = &r.turret;
+ auto *const turret_params = &turret->subsystem_params;
- // Measured absolute position of the encoder when at zero.
- intake->zeroing_constants.measured_absolute_position = 0.0;
+ turret_params->zeroing_voltage = 4.0;
+ turret_params->operating_voltage = 8.0;
+ turret_params->zeroing_profile_params = {0.5, 2.0};
+ turret_params->default_profile_params = {15.0, 40.0};
+ turret_params->range = Values::kTurretRange();
+ turret_params->make_integral_loop =
+ control_loops::superstructure::turret::MakeIntegralTurretLoop;
+ turret_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ turret_params->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kTurretEncoderRatio();
+ turret_params->zeroing_constants.zeroing_threshold = 0.0005;
+ turret_params->zeroing_constants.moving_buffer_size = 20;
+ turret_params->zeroing_constants.allowable_encoder_error = 0.9;
+ turret_params->zeroing_constants.measured_absolute_position = 0.0;
// Climber constants
- auto *const climber = &r->climber;
+ auto *const climber = &r.climber;
climber->subsystem_params.zeroing_voltage = 3.0;
climber->subsystem_params.operating_voltage = 12.0;
climber->subsystem_params.zeroing_profile_params = {0.5, 0.1};
@@ -73,18 +96,54 @@
// A set of constants for tests.
case 1:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kCompTeamNumber:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kPracticeTeamNumber:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kCodingRobotTeamNumber:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
default:
@@ -94,26 +153,7 @@
return r;
}
-const Values *values = nullptr;
-
-void DoGetValues() {
- uint16_t team = ::aos::network::GetTeamNumber();
- LOG(INFO) << "creating a Constants for team: " << team;
- values = DoGetValuesForTeam(team);
-}
-
-} // namespace
-
-void InitValues() {
- static absl::once_flag once;
- absl::call_once(once, DoGetValues);
-}
-
-const Values &GetValues() {
- CHECK(values)
- << "Values are uninitialized. Call InitValues before accessing them.";
- return *values;
-}
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
} // namespace constants
} // namespace y2022