Calibrate 2020 comp robot's sensors
Change-Id: Idfa19614f7044d0fc28494736eb685d28851fe33
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 90129dd..f0610a0 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -102,12 +102,15 @@
break;
case kCompTeamNumber:
- hood->zeroing_constants.measured_absolute_position = 0.0;
+ hood->zeroing_constants.measured_absolute_position = 0.432541963515072;
- intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position =
+ 1.42977866919024 - Values::kIntakeZero();
- turret->potentiometer_offset = 0.0;
- turret_params->zeroing_constants.measured_absolute_position = 0.0;
+ turret->potentiometer_offset =
+ 5.52519370141247 + 0.00853506822980376 + 0.0109413725126625;
+ turret_params->zeroing_constants.measured_absolute_position =
+ 0.251065633255048;
break;
case kPracticeTeamNumber:
diff --git a/y2020/constants.h b/y2020/constants.h
index 7fbb704..f0e3564 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -49,10 +49,10 @@
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- -0.01, // Back Hard
- 0.65, // Front Hard
- 0.0, // Back Soft
- 0.64 // Front Soft
+ 0.00, // Back Hard
+ 0.64, // Front Hard
+ 0.01, // Back Soft
+ 0.63 // Front Soft
};
}
@@ -71,16 +71,17 @@
kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
}
- // TODO(sabina): update range
static constexpr ::frc971::constants::Range kIntakeRange() {
return ::frc971::constants::Range{
- -1, // Back Hard
- 1, // Front Hard
- -0.95, // Back Soft
- 0.95 // Front Soft
+ -1.05, // Back Hard
+ 1.44, // Front Hard
+ -0.89, // Back Soft
+ 1.26 // Front Soft
};
}
+ static constexpr double kIntakeZero() { return -57 * M_PI / 180.0; }
+
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
intake;
@@ -92,7 +93,7 @@
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kTurretEncoderRatio() {
- return (26.0 / 150.0) * (130.0 / 40.0);
+ return (26.0 / 150.0) * (130.0 / 26.0);
}
static constexpr double kMaxTurretEncoderPulsesPerSecond() {
@@ -105,11 +106,10 @@
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
- // TODO (Kai): Placeholders right now.
- -3.2, // Back Hard
- 3.2, // Front Hard
- -3.14, // Back Soft
- 3.14 // Front Soft
+ -4.6, // Back Hard
+ 4.85, // Front Hard
+ -4.3, // Back Soft
+ 4.7123 // Front Soft
};
}
@@ -142,7 +142,7 @@
// Shooter
static constexpr double kFinisherEncoderCountsPerRevolution() {
- return 4096.0;
+ return 2048.0;
}
static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
@@ -154,7 +154,7 @@
static constexpr double kAcceleratorEncoderCountsPerRevolution() {
- return 4096.0;
+ return 2048.0;
}
static constexpr double kAcceleratorEncoderRatio() {
return (1.2 * 1.2 * 1.2) * (30.0 / 40.0);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 3cd391c..ee94dd6 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -672,8 +672,8 @@
superstructure_plant_.set_peak_turret_velocity(23.0);
superstructure_plant_.set_peak_turret_acceleration(0.2);
- // Intake needs over 8 seconds to reach the goal
- RunFor(chrono::seconds(9));
+ // Intake needs over 9 seconds to reach the goal
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index b336fd7..b5b6c1e 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -126,7 +126,6 @@
}
// Hood
-
void set_hood_encoder(::std::unique_ptr<frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
hood_encoder_.set_encoder(::std::move(encoder));
@@ -228,7 +227,7 @@
frc971::AbsolutePositionT hood;
CopyPosition(hood_encoder_, &hood,
Values::kHoodEncoderCountsPerRevolution(),
- Values::kHoodEncoderRatio(), false);
+ Values::kHoodEncoderRatio(), true);
flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
frc971::AbsolutePosition::Pack(*builder.fbb(), &hood);
@@ -244,7 +243,7 @@
frc971::PotAndAbsolutePositionT turret;
CopyPosition(turret_encoder_, &turret,
Values::kTurretEncoderCountsPerRevolution(),
- Values::kTurretEncoderRatio(), turret_pot_translate, false,
+ Values::kTurretEncoderRatio(), turret_pot_translate, true,
values.turret.potentiometer_offset);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
@@ -484,23 +483,25 @@
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
-
// TODO: pin numbers
- sensor_reader.set_hood_encoder(make_encoder(2));
- sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_hood_encoder(
+ make_unique<frc::Encoder>(22, 23, false, frc::Encoder::k4X));
- sensor_reader.set_intake_encoder(make_encoder(3));
- sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(24));
- sensor_reader.set_turret_encoder(make_encoder(4));
- sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(4));
- sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_intake_encoder(make_encoder(5));
+ sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(1));
- sensor_reader.set_flywheel_encoder(make_encoder(5));
- sensor_reader.set_left_kicker_encoder(make_encoder(6));
- sensor_reader.set_right_kicker_encoder(make_encoder(7));
+ sensor_reader.set_turret_encoder(make_encoder(2));
+ sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(0));
+ sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(0));
- sensor_reader.set_control_panel_encoder(make_encoder(8, frc::Encoder::k2X));
+ sensor_reader.set_finisher_encoder(
+ make_unique<frc::Encoder>(3, 2, false, frc::Encoder::k4X));
+ sensor_reader.set_left_accelerator_encoder(make_encoder(4));
+ sensor_reader.set_right_accelerator_encoder(make_encoder(3));
+
+ sensor_reader.set_control_panel_encoder(make_encoder(8, frc::Encoder::k1X));
// Note: If ADIS16470 is plugged in directly to the roboRIO SPI port without
// the Spartan Board, then trigger is on 26, reset 27, and chip select is
@@ -514,14 +515,13 @@
imu_reset = make_unique<frc::DigitalOutput>(27);
spi_port = frc::SPI::Port::kOnboardCS0;
} else {
- imu_trigger = make_unique<frc::DigitalInput>(0);
- imu_reset = make_unique<frc::DigitalOutput>(1);
+ imu_trigger = make_unique<frc::DigitalInput>(9);
+ imu_reset = make_unique<frc::DigitalOutput>(8);
}
auto spi = make_unique<frc::SPI>(spi_port);
frc971::wpilib::ADIS16470 imu(&sensor_reader_event_loop, spi.get(),
imu_trigger.get(), imu_reset.get());
sensor_reader.set_imu(&imu);
-
AddLoop(&sensor_reader_event_loop);
// Thread 4.