Merge "Add clear() function to priority_queue"
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 44eb262..18ed29e 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -188,3 +188,33 @@
// Maximum acceleration for the profile.
float max_acceleration;
};
+
+// Definition of a constraint on a trajectory
+struct Constraint {
+ // Type of constraint
+ // 0: Null constraint. Ignore and all following
+ // 1: longitual acceleration
+ // 2: lateral acceleration
+ // 3: voltage
+ // 4: velocity
+ uint8_t constraint_type;
+ float value;
+ // start and end distance are only checked for velocity limits.
+ float start_distance;
+ float end_distance;
+};
+
+// Parameters for computing a trajectory using a chain of splines and
+// constraints.
+struct MultiSpline {
+ // index of the spline. Zero indicates the spline should not be computed.
+ uint32_t spline_idx;
+ // Number of splines. The spline point arrays will be expected to have
+ // 6 + 5 * (n - 1) points in them. The endpoints are shared between
+ // neighboring splines.
+ uint8_t spline_count;
+ float[36] spline_x;
+ float[36] spline_y;
+
+ Constraint[6] constraints;
+};
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index aee3a37..d95e101 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -49,6 +49,23 @@
)
cc_library(
+ name = "splinedrivetrain",
+ srcs = [
+ "splinedrivetrain.cc",
+ ],
+ hdrs = [
+ "splinedrivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":spline",
+ ":distance_spline",
+ ":trajectory",
+ ]
+)
+
+cc_library(
name = "ssdrivetrain",
srcs = [
"ssdrivetrain.cc",
@@ -170,6 +187,7 @@
":gear",
":polydrivetrain",
":ssdrivetrain",
+ ":splinedrivetrain",
"//aos/controls:control_loop",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a50b429..168f81d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -38,6 +38,7 @@
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_spline_(dt_config_),
down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -329,12 +330,15 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
+ dt_spline_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
dt_closedloop_.Update(output != NULL && controller_type == 1);
+ dt_spline_.Update(output != NULL && controller_type == 2);
+
switch (controller_type) {
case 0:
dt_openloop_.SetOutput(output);
@@ -342,6 +346,9 @@
case 1:
dt_closedloop_.SetOutput(output);
break;
+ case 2:
+ dt_spline_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a357eab..05bf711 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
namespace frc971 {
namespace control_loops {
@@ -50,6 +51,7 @@
StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ SplineDrivetrain dt_spline_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index c66b03e..f63f685 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -72,6 +72,14 @@
// The control loop will profile if these are all non-zero.
.frc971.ProfileParameters linear;
.frc971.ProfileParameters angular;
+
+ // Parameters for a spline to follow. This just contains info on a spline to
+ // compute. Each time this is sent, spline drivetrain will compute a new
+ // spline.
+ .frc971.MultiSpline spline;
+
+ // Which spline to follow.
+ uint32_t spline_handle;
};
message Position {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
new file mode 100644
index 0000000..0d6b6ae
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -0,0 +1,101 @@
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+const int kMaxSplineConstraints = 6;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config) {}
+
+// TODO(alex): put in another thread to avoid malloc in RT.
+void SplineDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ current_spline_handle_ = goal.spline_handle;
+ const ::frc971::MultiSpline &multispline = goal.spline;
+ if (multispline.spline_idx) {
+ current_spline_idx_ = multispline.spline_idx;
+ auto x = multispline.spline_x;
+ auto y = multispline.spline_y;
+ ::std::vector<Spline> splines = ::std::vector<Spline>();
+ for (int i = 0; i < multispline.spline_count; ++i) {
+ ::Eigen::Matrix<double, 2, 6> points =
+ ::Eigen::Matrix<double, 2, 6>::Zero();
+ for (int j = 0; j < 6; ++j) {
+ points(0, j) = x[i * 5 + j];
+ points(1, j) = y[i * 5 + j];
+ }
+ splines.emplace_back(Spline(points));
+ }
+
+ distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+ new DistanceSpline(::std::move(splines)));
+
+ current_trajectory_ = ::std::unique_ptr<Trajectory>(
+ new Trajectory(distance_spline_.get(), dt_config_));
+
+ for (int i = 0; i < kMaxSplineConstraints; ++i) {
+ const ::frc971::Constraint &constraint = multispline.constraints[i];
+ switch (constraint.constraint_type) {
+ case 0:
+ break;
+ case 1:
+ current_trajectory_->set_longitudal_acceleration(constraint.value);
+ break;
+ case 2:
+ current_trajectory_->set_lateral_acceleration(constraint.value);
+ break;
+ case 3:
+ current_trajectory_->set_voltage_limit(constraint.value);
+ break;
+ case 4:
+ current_trajectory_->LimitVelocity(constraint.start_distance,
+ constraint.end_distance,
+ constraint.value);
+ break;
+ }
+ }
+
+ current_trajectory_->Plan();
+ current_xva_ = current_trajectory_->FFAcceleration(0);
+ current_xva_(1) = 0.0;
+ }
+}
+
+void SplineDrivetrain::Update(bool enable) {
+ if (enable && current_trajectory_ &&
+ !current_trajectory_->is_at_end(current_state_)) {
+ next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, ¤t_state_);
+ }
+}
+
+// TODO(alex): Handle drift.
+void SplineDrivetrain::SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ if (!output) {
+ return;
+ }
+ if (!current_trajectory_) {
+ return;
+ }
+ if (current_spline_handle_ == current_spline_idx_) {
+ if (!current_trajectory_->is_at_end(current_state_)) {
+ double current_distance = current_xva_(0);
+ ::Eigen::Matrix<double, 2, 1> FFVoltage =
+ current_trajectory_->FFVoltage(current_distance);
+ output->left_voltage = FFVoltage(0);
+ output->right_voltage = FFVoltage(1);
+ current_xva_ = next_xva_;
+ }
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
new file mode 100644
index 0000000..4bb7ac9
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -0,0 +1,44 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class SplineDrivetrain {
+ public:
+ SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
+
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+
+ void Update(bool enabled);
+
+ void SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output);
+ // TODO(alex): What status do we need?
+ void PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ private:
+ const DrivetrainConfig<double> dt_config_;
+
+ uint32_t current_spline_handle_; // Current spline told to excecute.
+ uint32_t current_spline_idx_; // Current excecuting spline.
+ ::std::unique_ptr<DistanceSpline> distance_spline_;
+ ::std::unique_ptr<Trajectory> current_trajectory_;
+ ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
+ ::Eigen::Matrix<double, 2, 1> current_state_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 16cdfd9..b53fb99 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -145,7 +145,7 @@
elevator_params->zeroing_constants.measured_absolute_position = 0.049419;
elevator->potentiometer_offset = -0.022320;
- intake->zeroing_constants.measured_absolute_position = 2.303729;
+ intake->zeroing_constants.measured_absolute_position = 1.756847;
intake->zeroing_constants.middle_position =
Values::kIntakeRange().middle();
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.cc b/y2019/control_loops/drivetrain/drivetrain_base.cc
index 26ca861..02bd805 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_base.cc
@@ -40,8 +40,8 @@
drivetrain::kMass, kThreeStateDriveShifter, kThreeStateDriveShifter,
true /* default_high_gear */, 0 /* down_offset if using constants use
constants::GetValues().down_error */,
- 0.8 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
- 1.5 /* wheel_multiplier */,
+ 0.7 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
+ 1.2 /* wheel_multiplier */,
};
return kDrivetrainConfig;
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 6646f96..5edfce3 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -50,11 +50,10 @@
output != nullptr ? &(output->wrist_voltage) : nullptr,
&(status->wrist));
- intake_.Iterate(
- unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake_joint),
+ output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+ &(status->intake));
stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
&(position->stilts),
@@ -67,6 +66,16 @@
status->estopped = status->elevator.estopped || status->wrist.estopped ||
status->intake.estopped || status->stilts.estopped;
+ if (output) {
+ if (status->intake.position > kMinIntakeAngleForRollers) {
+ output->intake_roller_voltage =
+ (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
+
+ } else {
+ output->intake_roller_voltage = 0.0;
+ }
+ }
+
// TODO(theo) move these up when Iterate() is split
// update the goals
collision_avoidance_.UpdateGoal(status, unsafe_goal);
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 22dbcd6..c3e53f2 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -28,18 +28,10 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
- const PotAndAbsoluteEncoderSubsystem &elevator() const {
- return elevator_;
- }
- const PotAndAbsoluteEncoderSubsystem &wrist() const {
- return wrist_;
- }
- const AbsoluteEncoderSubsystem &intake() const {
- return intake_;
- }
- const PotAndAbsoluteEncoderSubsystem &stilts() const {
- return stilts_;
- }
+ const PotAndAbsoluteEncoderSubsystem &elevator() const { return elevator_; }
+ const PotAndAbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+ const AbsoluteEncoderSubsystem &intake() const { return intake_; }
+ const PotAndAbsoluteEncoderSubsystem &stilts() const { return stilts_; }
protected:
virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
@@ -55,6 +47,8 @@
CollisionAvoidance collision_avoidance_;
+ static constexpr double kMinIntakeAngleForRollers = -0.7;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index de973c0..bd18b60 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -342,9 +342,9 @@
CheckCollisions();
}
- const double loop_time = chrono::duration_cast<chrono::duration<double>>(
- monotonic_clock::now() - loop_start_time)
- .count();
+ const double loop_time =
+ chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time).count();
const double elevator_acceleration =
(superstructure_plant_.elevator_velocity() -
@@ -657,6 +657,52 @@
VerifyNearGoal();
}
+// Tests that the rollers spin when allowed
+TEST_F(SuperstructureTest, IntakeRollerTest) {
+ WaitUntilZeroed();
+
+ // Get the elevator and wrist out of the way and set the Intake to where
+ // we should be able to spin and verify that they do
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage,
+ superstructure_queue_.goal->roller_voltage);
+ VerifyNearGoal();
+
+ // Move the intake where we oughtn't to spin the rollers and verify they don't
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage, 0.0);
+ VerifyNearGoal();
+}
+
+// Tests that running disabled, ya know, works
+TEST_F(SuperstructureTest, DiasableTest) {
+ RunForTime(chrono::seconds(2), false, false);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops