Merge changes I8e9a3a44,I0e992ccf,I53881cf2
* changes:
Recal pot after weird pot slip event
Add ability to wait for a distance along a spline in auto
Fix arm freakout
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 90c1454..22909be 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -450,6 +450,35 @@
return false;
}
+bool BaseAutonomousActor::SplineHandle::SplineDistanceTraveled(
+ double distance) {
+ base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
+ if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
+ // Confirm that:
+ // (a) The spline has started executiong (is_executing remains true even
+ // when we reach the end of the spline).
+ // (b) The spline that we are executing is the correct one.
+ // (c) There is less than distance distance remaining.
+ if (base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->goal_spline_handle() != spline_handle_) {
+ // Never done if we aren't the active spline.
+ return false;
+ }
+
+ if (base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->is_executed()) {
+ return true;
+ }
+ return base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging()
+ ->is_executing() &&
+ base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging()
+ ->distance_traveled() > distance;
+ }
+ return false;
+}
+
bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
double distance) {
::aos::time::PhasedLoop phased_loop(
@@ -467,6 +496,23 @@
}
}
+bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceTraveled(
+ double distance) {
+ ::aos::time::PhasedLoop phased_loop(
+ frc971::controls::kLoopFrequency,
+ base_autonomous_actor_->event_loop()->monotonic_now(),
+ ActorBase::kLoopOffset);
+ while (true) {
+ if (base_autonomous_actor_->ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ if (SplineDistanceTraveled(distance)) {
+ return true;
+ }
+ }
+}
+
void BaseAutonomousActor::LineFollowAtVelocity(
double velocity, y2019::control_loops::drivetrain::SelectionHint hint) {
{
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 5562dde..403854f 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -41,7 +41,9 @@
// Whether there is less than a certain distance, in meters, remaining in
// the current spline.
bool SplineDistanceRemaining(double distance);
+ bool SplineDistanceTraveled(double distance);
bool WaitForSplineDistanceRemaining(double distance);
+ bool WaitForSplineDistanceTraveled(double distance);
// Returns [x, y, theta] position of the start.
const Eigen::Vector3d &starting_position() const { return spline_start_; }
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 89dc3d5..8158e75 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -77,6 +77,7 @@
left_velocity:float (id: 9);
right_velocity:float (id: 10);
distance_remaining:float (id: 11);
+ distance_traveled:float (id: 13);
// Splines that we have full plans for.
available_splines:[int] (id: 12);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0f9418c..ddcf63d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -256,6 +256,8 @@
? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
: 0.0);
trajectory_logging_builder.add_available_splines(handles_vector);
+ trajectory_logging_builder.add_distance_traveled(
+ executing_spline_ ? current_xva_.x() : 0.0);
return trajectory_logging_builder.Finish();
}
diff --git a/y2023/constants.cc b/y2023/constants.cc
index b64ac3d..3d3dfc2 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -95,7 +95,8 @@
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
0.0164398318919943 - 0.145833494945215 + 0.234878799868491 +
0.125924230298394 + 0.147136306208754 - 0.69167546169753 -
- 0.308761538844425 + 0.610386472488493 + 0.08384162885249 + 0.0262274735196811;
+ 0.308761538844425 + 0.610386472488493 + 0.08384162885249 +
+ 0.0262274735196811 + 0.5153995156153;
arm_distal->zeroing.one_revolution_distance =
M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index d216467..88e3d1b 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -974,8 +974,8 @@
}
// Pull us back to the previous point until we aren't saturated anymore.
- double saturation_goal_velocity;
- double saturation_goal_acceleration;
+ double saturation_goal_velocity = 0.0;
+ double saturation_goal_acceleration = 0.0;
while (step_size > 0.01) {
USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
saturation_fraction_along_path_, arm_K, X, *trajectory_,
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
index c56ced9..24bc8dd 100644
--- a/y2023/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -15,10 +15,10 @@
DEFINE_bool(plot, true, "If true, plot");
DEFINE_bool(plot_thetas, true, "If true, plot the angles");
-DEFINE_double(alpha0_max, 20.0, "Max acceleration on joint 0.");
-DEFINE_double(alpha1_max, 30.0, "Max acceleration on joint 1.");
-DEFINE_double(alpha2_max, 60.0, "Max acceleration on joint 2.");
-DEFINE_double(vmax_plan, 10.0, "Max voltage to plan.");
+DEFINE_double(alpha0_max, 15.0, "Max acceleration on joint 0.");
+DEFINE_double(alpha1_max, 10.0, "Max acceleration on joint 1.");
+DEFINE_double(alpha2_max, 90.0, "Max acceleration on joint 2.");
+DEFINE_double(vmax_plan, 9.5, "Max voltage to plan.");
DEFINE_double(vmax_battery, 12.0, "Max battery voltage.");
DEFINE_double(time, 2.0, "Simulation time.");
@@ -36,12 +36,20 @@
Eigen::Matrix<double, 2, 4> spline_params;
- spline_params << 0.30426338, 0.42813912, 0.64902386, 0.55127045, -1.73611082,
- -1.64478944, -1.04763868, -0.82624244;
+ spline_params << 3.227752818257, 3.032002509469, 3.131082488348,
+ 3.141592653590, 0.914286433787, 0.436747899287, 0.235917057271,
+ 0.000000000000;
LOG(INFO) << "Spline " << spline_params;
NSpline<4, 2> spline(spline_params);
CosSpline cos_spline(spline,
- {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+ {
+ CosSpline::AlphaTheta{.alpha = 0.000000000000,
+ .theta = 1.570796326795},
+ CosSpline::AlphaTheta{.alpha = 0.050000000000,
+ .theta = 1.570796326795},
+ CosSpline::AlphaTheta{.alpha = 1.000000000000,
+ .theta = 0.000000000000},
+ });
Path distance_spline(cos_spline, 100);
Trajectory trajectory(&dynamics, &hybrid_roll.plant(),
@@ -300,10 +308,14 @@
(::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
sim_dt);
roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+ bool disabled = false;
+ if (t > 0.40 && t < 0.46) {
+ disabled = true;
+ }
follower.Update(
(Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
.finished(),
- false, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
+ disabled, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
const ::Eigen::Matrix<double, 3, 1> theta_t =
trajectory.ThetaT(follower.goal()(0));