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.h b/y2022/constants.h
index 6927567..65aad44 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,7 +9,9 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/superstructure/climber/climber_plant.h"
#include "y2022/control_loops/superstructure/intake/intake_plant.h"
+#include "y2022/control_loops/superstructure/turret/turret_plant.h"
namespace y2022 {
namespace constants {
@@ -35,35 +37,13 @@
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
(2.0 * M_PI)) *
- kDrivetrainEncoderRatio() *
- control_loops::drivetrain::kWheelRadius;
- }
-
- static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
- static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
-
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- intake;
-
- // TODO (Yash): Constants need to be tuned
- static constexpr ::frc971::constants::Range kIntakeRange() {
- return ::frc971::constants::Range{
- .lower_hard = -0.5, // Back Hard
- .upper_hard = 2.85 + 0.05, // Front Hard
- .lower = -0.300, // Back Soft
- .upper = 2.725 // Front Soft
- };
+ kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
}
// Climber
static constexpr ::frc971::constants::Range kClimberRange() {
return ::frc971::constants::Range{
- .lower_hard = -0.01,
- .upper_hard = 0.6,
- .lower = 0.0,
- .upper = 0.5
- };
+ .lower_hard = -0.01, .upper_hard = 0.6, .lower = 0.0, .upper = 0.5};
}
static constexpr double kClimberPotMetersPerRevolution() {
return 22 * 0.25 * 0.0254;
@@ -81,23 +61,76 @@
// Intake
// two encoders with same gear ratio for intake
- static constexpr double kIntakeEncoderCountsPerRevolution() { return 512.0; }
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kIntakeEncoderRatio() {
- return ((16.0 / 60.0) * (18.0 / 62.0));
+ return (16.0 / 64.0) * (20.0 / 50.0);
}
- // TODO(Milo): Also need to add specific PPR (Pulse per revolution)
+ static constexpr double kIntakePotRatio() { return 16.0 / 64.0; }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
+ }
+
+ struct PotAndAbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+
+ PotAndAbsEncoderConstants intake_front;
+ PotAndAbsEncoderConstants intake_back;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.5, // Back Hard
+ .upper_hard = 2.85 + 0.05, // Front Hard
+ .lower = -0.300, // Back Soft
+ .upper = 2.725 // Front Soft
+ };
+ }
+
+ // Intake rollers
+ static constexpr double kIntakeRollerSupplyCurrentLimit() { return 40.0; }
+ static constexpr double kIntakeRollerStatorCurrentLimit() { return 60.0; }
+
+ // Turret
+ PotAndAbsEncoderConstants turret;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kTurretRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -3.45, // Back Hard
+ .upper_hard = 3.45, // Front Hard
+ .lower = -3.3, // Back Soft
+ .upper = 3.3 // Front Soft
+ };
+ }
+
+ // Turret
+ static constexpr double kTurretPotRatio() { return 27.0 / 110.0; }
+ static constexpr double kTurretEncoderRatio() { return kTurretPotRatio(); }
+ static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kMaxTurretEncoderPulsesPerSecond() {
+ return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::turret::kOutputRatio /
+ kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
+ }
};
-// Creates (once) a Values instance for ::aos::network::GetTeamNumber(). Should
-// be called before realtime because this allocates memory.
-void InitValues();
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+Values MakeValues(uint16_t team);
-// Returns a reference to the Values instance for
-// ::aos::network::GetTeamNumber(). Values must be initialized through
-// InitValues() before calling this.
-const Values &GetValues();
+// Calls MakeValues with aos::network::GetTeamNumber()
+Values MakeValues();
} // namespace constants
} // namespace y2022