Update Hood to use AbsoluteAndAbsolute position
Change-Id: I3e88718560a6e3983681a3430dc31b5431c1a743
diff --git a/y2020/constants.cc b/y2020/constants.cc
index d39e1cd..0e36f0f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -31,8 +31,8 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const hood =
- &r->hood;
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
+ *const hood = &r->hood;
// Hood constants.
hood->zeroing_voltage = 3.0;
@@ -43,12 +43,17 @@
hood->make_integral_loop =
control_loops::superstructure::hood::MakeIntegralHoodLoop;
hood->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
- hood->zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kHoodEncoderRatio();
hood->zeroing_constants.zeroing_threshold = 0.0005;
hood->zeroing_constants.moving_buffer_size = 20;
hood->zeroing_constants.allowable_encoder_error = 0.9;
- hood->zeroing_constants.middle_position = Values::kHoodRange().middle();
+ hood->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kHoodEncoderRatio();
+ hood->zeroing_constants.single_turn_middle_position =
+ Values::kHoodRange().middle();
+ hood->zeroing_constants.single_turn_one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kHoodSingleTurnEncoderRatio();
+ hood->zeroing_constants.measured_absolute_position = 0;
+ hood->zeroing_constants.single_turn_measured_absolute_position = 0;
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
@@ -93,7 +98,7 @@
turret_params->zeroing_constants.allowable_encoder_error = 0.9;
CHECK_LE(hood->range.range(),
- hood->zeroing_constants.one_revolution_distance);
+ hood->zeroing_constants.single_turn_one_revolution_distance);
CHECK_LE(intake->range.range(),
intake->zeroing_constants.one_revolution_distance);
@@ -113,6 +118,9 @@
0.0109413725126625 - 0.223719825811759;
turret_params->zeroing_constants.measured_absolute_position =
0.547478339799516;
+
+ hood->zeroing_constants.measured_absolute_position = 0.03207;
+ hood->zeroing_constants.single_turn_measured_absolute_position = 0.31055;
break;
case kPracticeTeamNumber:
diff --git a/y2020/constants.h b/y2020/constants.h
index f0e3564..81e6dd0 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -39,7 +39,20 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kHoodEncoderRatio() { return 8.0 / 72.0; }
+ static constexpr double kHoodEncoderRatio() {
+ // TODO: This math is not quite right
+ // 10.211 in of travel gets you 1 radian on the output
+ const double radians_per_in_travel = 1.0 / 10.211;
+
+ // one turn on the leadscrew gets you 12.0 mm travel
+ const double mm_travel_per_radian = 12.0 / (2.0 * M_PI);
+ const double in_travel_per_radian = mm_travel_per_radian / 25.4;
+
+ // units reduce; radians on the encoder * this number = radians on the hood
+ return in_travel_per_radian * radians_per_in_travel / 0.94816;
+ }
+
+ static constexpr double kHoodSingleTurnEncoderRatio() { return 8.0 / 72.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
return control_loops::superstructure::hood::kFreeSpeed / (2.0 * M_PI) *
@@ -50,14 +63,14 @@
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
0.00, // Back Hard
- 0.64, // Front Hard
- 0.01, // Back Soft
- 0.63 // Front Soft
+ 0.64, // Front Hard
+ 0.01, // Back Soft
+ 0.63 // Front Soft
};
}
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
hood;
// Intake
@@ -152,7 +165,6 @@
kFinisherEncoderRatio() * kFinisherEncoderCountsPerRevolution();
}
-
static constexpr double kAcceleratorEncoderCountsPerRevolution() {
return 2048.0;
}
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index d8118a1..db6b727 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -20,17 +20,19 @@
# Hood is an angular subsystem due to the mounting of the encoder on the hood
# joint. We are currently electing to ignore potential non-linearity.
-range_of_travel_radians = (37.0 * numpy.pi / 180.0)
-# 0.083 inches/turn
-# 6.38 inches of travel
-turns_of_leadscrew_per_range_of_travel = 6.38 / 0.083
+range_of_travel_radians = (38.0 * numpy.pi / 180.0)
+# 0.472441 inches/turn (12 mm)
+# 6.7725 inches of travel
+turns_of_leadscrew_per_range_of_travel = 6.7725 / 0.472441
radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
+radians_per_turn_of_motor = 12.0 / 60.0 * radians_per_turn
+
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- G=radians_per_turn / (2.0 * numpy.pi),
+ G=radians_per_turn_of_motor / (2.0 * numpy.pi),
J=4.0,
q_pos=0.15,
q_vel=5.0,
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 4c02a18..e6c1375 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -6,6 +6,7 @@
namespace control_loops {
namespace superstructure {
+using frc971::control_loops::AbsoluteAndAbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
@@ -65,11 +66,12 @@
OutputT output_struct;
- flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
- hood_.Iterate(unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
- position->hood(),
- output != nullptr ? &(output_struct.hood_voltage) : nullptr,
- status->fbb());
+ flatbuffers::Offset<AbsoluteAndAbsoluteEncoderProfiledJointStatus>
+ hood_status_offset = hood_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+ position->hood(),
+ output != nullptr ? &(output_struct.hood_voltage) : nullptr,
+ status->fbb());
if (unsafe_goal != nullptr) {
if (unsafe_goal->shooting() &&
@@ -121,7 +123,7 @@
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
- const AbsoluteEncoderProfiledJointStatus *const hood_status =
+ const AbsoluteAndAbsoluteEncoderProfiledJointStatus *const hood_status =
GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 7b87bca..c0f3798 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -6,12 +6,12 @@
#include "aos/robot_state/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/climber.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2020/control_loops/superstructure/climber.h"
#include "y2020/control_loops/superstructure/turret/aiming.h"
namespace y2020 {
@@ -40,8 +40,13 @@
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::
+ AbsoluteAndAbsoluteEncoderProfiledJointStatus>;
- const AbsoluteEncoderSubsystem &hood() const { return hood_; }
+ const AbsoluteAndAbsoluteEncoderSubsystem &hood() const { return hood_; }
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
const shooter::Shooter &shooter() const { return shooter_; }
@@ -53,7 +58,7 @@
aos::Sender<Status>::Builder *status) override;
private:
- AbsoluteEncoderSubsystem hood_;
+ AbsoluteAndAbsoluteEncoderSubsystem hood_;
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
shooter::Shooter shooter_;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index ac1e4a0..cb4ea64 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -38,6 +38,8 @@
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
typedef ::frc971::control_loops::drivetrain::Status DrivetrainStatus;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteAndAbsoluteEncoderSubsystem
+ AbsoluteAndAbsoluteEncoderSubsystem;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
@@ -88,8 +90,11 @@
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
- hood_encoder_(constants::GetValues()
- .hood.zeroing_constants.one_revolution_distance),
+ hood_encoder_(
+ constants::GetValues()
+ .hood.zeroing_constants.one_revolution_distance,
+ constants::GetValues()
+ .hood.zeroing_constants.single_turn_one_revolution_distance),
intake_plant_(new CappedTestPlant(intake::MakeIntakePlant())),
intake_encoder_(constants::GetValues()
.intake.zeroing_constants.one_revolution_distance),
@@ -129,7 +134,9 @@
hood_encoder_.Initialize(
start_pos, kNoiseScalar, 0.0,
constants::GetValues()
- .hood.zeroing_constants.measured_absolute_position);
+ .hood.zeroing_constants.measured_absolute_position,
+ constants::GetValues()
+ .hood.zeroing_constants.single_turn_measured_absolute_position);
}
void InitializeIntakePosition(double start_pos) {
@@ -165,9 +172,9 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
- frc971::AbsolutePosition::Builder hood_builder =
- builder.MakeBuilder<frc971::AbsolutePosition>();
- flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
+ frc971::AbsoluteAndAbsolutePosition::Builder hood_builder =
+ builder.MakeBuilder<frc971::AbsoluteAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsoluteAndAbsolutePosition> hood_offset =
hood_encoder_.GetSensorValues(&hood_builder);
frc971::AbsolutePosition::Builder intake_builder =
@@ -224,9 +231,9 @@
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
const double voltage_check_hood =
- (static_cast<AbsoluteEncoderSubsystem::State>(
+ (static_cast<AbsoluteAndAbsoluteEncoderSubsystem::State>(
superstructure_status_fetcher_->hood()->state()) ==
- AbsoluteEncoderSubsystem::State::RUNNING)
+ AbsoluteAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().hood.operating_voltage
: constants::GetValues().hood.zeroing_voltage;
@@ -826,7 +833,7 @@
SetEnabled(true);
WaitUntilZeroed();
RunFor(chrono::seconds(2));
- EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
+ EXPECT_EQ(AbsoluteAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.hood().state());
EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
index f139381..1e9f81a 100644
--- a/y2020/control_loops/superstructure/superstructure_position.fbs
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -14,7 +14,7 @@
table Position {
// Zero is at the horizontal, positive towards the front (radians).
- hood:frc971.AbsolutePosition (id: 0);
+ hood:frc971.AbsoluteAndAbsolutePosition (id: 0);
// Position of the intake. 0 when four-bar is vertical, positive extended.
intake_joint:frc971.AbsolutePosition (id: 1);
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index fb94d32..83ee607 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -51,7 +51,7 @@
estopped:bool (id: 1);
// Subsystem status.
- hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 2);
+ hood:frc971.control_loops.AbsoluteAndAbsoluteEncoderProfiledJointStatus (id: 2);
intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 4f39acf..e7b7b86 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -136,6 +136,11 @@
hood_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
}
+ void set_hood_single_turn_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ hood_encoder_.set_single_turn_absolute_pwm(::std::move(absolute_pwm));
+ }
+
// Intake
void set_intake_encoder(::std::unique_ptr<frc::Encoder> encoder) {
@@ -221,12 +226,13 @@
// TODO(alex): check new absolute encoder api.
// Hood
- frc971::AbsolutePositionT hood;
+ frc971::AbsoluteAndAbsolutePositionT hood;
CopyPosition(hood_encoder_, &hood,
Values::kHoodEncoderCountsPerRevolution(),
- Values::kHoodEncoderRatio(), true);
- flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
- frc971::AbsolutePosition::Pack(*builder.fbb(), &hood);
+ Values::kHoodEncoderRatio(),
+ Values::kHoodSingleTurnEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsoluteAndAbsolutePosition> hood_offset =
+ frc971::AbsoluteAndAbsolutePosition::Pack(*builder.fbb(), &hood);
// Intake
frc971::AbsolutePositionT intake_joint;
@@ -311,7 +317,9 @@
::frc971::wpilib::AbsoluteEncoderAndPotentiometer turret_encoder_;
- ::frc971::wpilib::AbsoluteEncoder hood_encoder_, intake_joint_encoder_;
+ ::frc971::wpilib::AbsoluteAndAbsoluteEncoder hood_encoder_;
+
+ ::frc971::wpilib::AbsoluteEncoder intake_joint_encoder_;
::std::unique_ptr<::frc::Encoder> finisher_encoder_,
left_accelerator_encoder_, right_accelerator_encoder_,
@@ -486,10 +494,13 @@
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
// TODO: pin numbers
+ // TODO(Ravago): Hood pin numbers
sensor_reader.set_hood_encoder(
make_unique<frc::Encoder>(22, 23, false, frc::Encoder::k4X));
- sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(24));
+ sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(25));
+ sensor_reader.set_hood_single_turn_absolute_pwm(
+ make_unique<frc::DigitalInput>(24));
sensor_reader.set_intake_encoder(make_encoder(5));
sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(1));