Merge changes I2790c09e,I286599e2,Ic15084e8
* changes:
Make drivetrain use generic localizer
Implement 2019 EKF
Add generic drivetrain EKF
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 4204b5c..d77d2c7 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -104,6 +104,7 @@
intake->zeroing_constants.zeroing_threshold = 0.0005;
intake->zeroing_constants.moving_buffer_size = 20;
intake->zeroing_constants.allowable_encoder_error = 0.9;
+ intake->zeroing_constants.middle_position = Values::kIntakeRange().middle();
// Stilts constants.
stilts_params->zeroing_voltage = 3.0;
@@ -128,7 +129,6 @@
elevator->potentiometer_offset = 0.0;
intake->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.middle_position = 0.0;
wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
@@ -138,29 +138,23 @@
break;
case kCompTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.0;
- elevator->potentiometer_offset = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.019470;
+ elevator->potentiometer_offset = -0.075017;
- intake->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 1.860016;
- wrist_params->zeroing_constants.measured_absolute_position = 0.0;
- wrist->potentiometer_offset = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.163840;
+ wrist->potentiometer_offset = -4.257454;
- stilts_params->zeroing_constants.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
+ stilts_params->zeroing_constants.measured_absolute_position = 0.030049;
+ stilts->potentiometer_offset = -0.015760 + 0.011604;
break;
case kPracticeTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.172663;
- elevator->potentiometer_offset = -0.022320 + 0.020567;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.160767;
+ elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355;
intake->zeroing_constants.measured_absolute_position = 1.756847;
- intake->zeroing_constants.middle_position =
- Values::kIntakeRange().middle();
-
- stilts_params->zeroing_constants.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
wrist_params->zeroing_constants.measured_absolute_position = 0.357394;
wrist->potentiometer_offset = -1.479097 - 2.740303;
@@ -174,7 +168,6 @@
elevator->potentiometer_offset = 0.0;
intake->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.middle_position = 0.0;
wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index 599e2de..3fd5a49 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -17,18 +17,18 @@
// Voltage to the vaccum pump when we are attempting to acquire a piece
- static constexpr double kPumpVoltage = 12.0;
+ static constexpr double kPumpVoltage = 8.0;
// Voltage to the vaccum pump when we have a piece
- static constexpr double kPumpHasPieceVoltage = 8.0;
+ static constexpr double kPumpHasPieceVoltage = 2.0;
// Time to continue at the higher pump voltage after getting a gamepiece
static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
- std::chrono::milliseconds(200);
+ std::chrono::milliseconds(100);
// Time to continue the pump after getting a no suck goal
static constexpr aos::monotonic_clock::duration kTimeToKeepPumpRunning =
- std::chrono::milliseconds(1000);
+ std::chrono::milliseconds(750);
private:
bool had_piece_ = false;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index b3969fb..2efb55b 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -23,11 +23,11 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
@@ -311,9 +311,17 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- suction_victor_->SetSpeed(
- ::aos::Clip(queue->pump_voltage, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ ::aos::robot_state.FetchLatest();
+ const double battery_voltage =
+ ::aos::robot_state.get() ? ::aos::robot_state->voltage_battery : 12.0;
+
+ // Throw a fast low pass filter on the battery voltage so we don't respond
+ // too fast to noise.
+ filtered_battery_voltage_ =
+ 0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
+
+ suction_victor_->SetSpeed(::aos::Clip(
+ queue->pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -328,6 +336,8 @@
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
wrist_victor_, stilts_victor_, suction_victor_;
+
+ double filtered_battery_voltage_ = 12.0;
};
class SolenoidWriter {
@@ -349,7 +359,7 @@
void set_intake_roller_talon(
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX> t) {
intake_rollers_talon_ = ::std::move(t);
- intake_rollers_talon_->ConfigContinuousCurrentLimit(40.0, 0);
+ intake_rollers_talon_->ConfigContinuousCurrentLimit(20.0, 0);
intake_rollers_talon_->EnableCurrentLimit(true);
}