Merge "Require the robot to see the platform before retracting the stilts"
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 061f4dc..6a38967 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -23,7 +23,8 @@
}
}
- if (!auto_running_ || (run_teleop_in_auto_ && !action_queue_.Running())) {
+ if (!auto_running_ ||
+ (input_config_.run_teleop_in_auto && !action_queue_.Running())) {
if (auto_was_running_) {
AutoEnded();
auto_was_running_ = false;
@@ -36,6 +37,11 @@
HandleTeleop(data);
}
+ if (auto_action_running_ &&
+ data.IsPressed(input_config_.cancel_auto_button)) {
+ StopAuto();
+ }
+
// Process pending actions.
action_queue_.Tick();
was_running_ = action_queue_.Running();
@@ -45,11 +51,13 @@
LOG(INFO, "Starting auto mode\n");
action_queue_.EnqueueAction(
::frc971::autonomous::MakeAutonomousAction(GetAutonomousMode()));
+ auto_action_running_ = true;
}
void ActionJoystickInput::StopAuto() {
LOG(INFO, "Stopping auto mode\n");
action_queue_.CancelAllActions();
+ auto_action_running_ = false;
}
} // namespace input
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index dca4d39..bb4e563 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -14,21 +14,29 @@
// Turns out we do the same thing every year, so let's stop copying it.
class ActionJoystickInput : public ::aos::input::JoystickInput {
public:
+ // Configuration parameters that don't really belong in the DrivetrainConfig.
+ struct InputConfig {
+ // If true, we will run teleop during auto mode after auto mode ends. This
+ // is to support the 2019 sandstorm mode. Auto will run, and then when the
+ // action ends (either when it's done, or when the driver triggers it to
+ // finish early), we will run teleop regardless of the mode.
+ bool run_teleop_in_auto = false;
+ // A button, for use with the run_teleop_in_auto, that will cancel the auto
+ // mode, and if run_telop_in_auto is specified, resume teloperation.
+ const driver_station::ButtonLocation cancel_auto_button;
+ };
ActionJoystickInput(
::aos::EventLoop *event_loop,
- const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config)
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+ dt_config,
+ const InputConfig &input_config)
: ::aos::input::JoystickInput(event_loop),
+ input_config_(input_config),
drivetrain_input_reader_(DrivetrainInputReader::Make(
DrivetrainInputReader::InputType::kPistol, dt_config)) {}
virtual ~ActionJoystickInput() {}
- protected:
- void set_run_teleop_in_auto(bool run_teleop_in_auto) {
- run_teleop_in_auto_ = run_teleop_in_auto;
- }
-
private:
// Handles anything that needs to be cleaned up when the auto action exits.
virtual void AutoEnded() {}
@@ -46,19 +54,16 @@
// True if the internal state machine thinks auto is running right now.
bool auto_running_ = false;
+ // True if we think that the auto *action* is running right now.
+ bool auto_action_running_ = false;
// True if an action was running last cycle.
bool was_running_ = false;
- // If true, we will run teleop during auto mode after auto mode ends. This is
- // to support the 2019 sandstorm mode. Auto will run, and then when the
- // action ends (either when it's done, or when the driver triggers it to
- // finish early), we will run teleop regardless of the mode.
- bool run_teleop_in_auto_ = false;
+ const InputConfig input_config_;
// Bool to track if auto was running the last cycle through. This lets us
// call AutoEnded when the auto mode function stops.
bool auto_was_running_ = false;
-
::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
::aos::common::actions::ActionQueue action_queue_;
};
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index fc01edc..86349de 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -364,7 +364,7 @@
drivetrain_message->spline = spline;
drivetrain_message->spline.spline_idx = spline_handle;
drivetrain_message->spline_handle = goal_spline_handle_;
- drivetrain_message->drive_spline_backwards =
+ drivetrain_message->spline.drive_spline_backwards =
direction == SplineDirection::kBackward;
LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
@@ -421,14 +421,14 @@
LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
// We check that the spline we are waiting on is neither currently planning
- // nor executing; note that we do *not* check the is_executing bit because
- // immediately after calling Start we may still receive an old Status message
- // that has not been updated. We check for planning so that the user can go
- // straight from starting the planner to executing without a WaitForPlan in
- // between.
+ // nor executing (we check is_executed because it is possible to receive
+ // status messages with is_executing false before the execution has started).
+ // We check for planning so that the user can go straight from starting the
+ // planner to executing without a WaitForPlan in between.
if (drivetrain_queue.status.get() &&
- (drivetrain_queue.status->trajectory_logging.current_spline_idx ==
- spline_handle_ ||
+ ((!drivetrain_queue.status->trajectory_logging.is_executed &&
+ drivetrain_queue.status->trajectory_logging.current_spline_idx ==
+ spline_handle_) ||
drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
spline_handle_)) {
return false;
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index a8e6b8c..2359bcd 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -216,5 +216,8 @@
float[36] spline_x;
float[36] spline_y;
+ // Whether to follow the spline driving forwards or backwards.
+ bool drive_spline_backwards;
+
Constraint[6] constraints;
};
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 09ecb4f..939ad75 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -45,6 +45,8 @@
// State of the spline execution.
bool is_executing;
+ // Whether we have finished the spline specified by current_spline_idx.
+ bool is_executed;
// The handle of the goal spline. 0 means stop requested.
int32_t goal_spline_handle;
@@ -130,8 +132,6 @@
// Which spline to follow.
int32_t spline_handle;
- // Whether to follow the spline driving forwards or backwards.
- bool drive_spline_backwards;
};
message Position {
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9d031b7..81506f2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -136,7 +136,7 @@
do {
RunIteration();
my_drivetrain_queue_.status.FetchLatest();
- } while (my_drivetrain_queue_.status->trajectory_logging.is_executing);
+ } while (!my_drivetrain_queue_.status->trajectory_logging.is_executed);
}
virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
@@ -439,10 +439,10 @@
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
my_drivetrain_queue_.goal.MakeMessage();
- goal->drive_spline_backwards = true;
goal->controller_type = 2;
goal->spline.spline_idx = 1;
goal->spline.spline_count = 1;
+ goal->spline.drive_spline_backwards = true;
goal->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
goal->spline.spline_y = {{0.0, 0.0, -0.25 ,-0.75, -1.0, -1.0}};
goal.Send();
@@ -455,8 +455,26 @@
start_goal.Send();
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ // Check that we are right on the spline at the start (otherwise the feedback
+ // will tend to correct for going the wrong direction).
+ for (int ii = 0; ii < 10; ++ii) {
+ RunForTime(chrono::milliseconds(100));
+ VerifyNearSplineGoal();
+ }
+
+ WaitForTrajectoryExecution();
+
VerifyNearSplineGoal();
+ // Check that we are pointed the right direction:
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.state();
+ const double expected_theta =
+ my_drivetrain_queue_.status->trajectory_logging.theta;
+ // As a sanity check, compare both against absolute angle and the spline's
+ // goal angle.
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta),
+ 2e-2);
}
// Tests that simple spline with a single goal message.
@@ -697,6 +715,46 @@
VerifyNearSplineGoal();
}
+// Tests that we can run a second spline after having planned but never executed
+// the first spline.
+TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ // Don't start running the splane.
+ goal->spline_handle = 0;
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(1000));
+
+ // Plan another spline, but don't start it yet:
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
+ second_spline_goal->controller_type = 2;
+ second_spline_goal->spline.spline_idx = 2;
+ second_spline_goal->spline.spline_count = 1;
+ second_spline_goal->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
+ second_spline_goal->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
+ second_spline_goal->spline_handle = 0;
+ second_spline_goal.Send();
+ WaitForTrajectoryPlan();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ execute_goal = my_drivetrain_queue_.goal.MakeMessage();
+ execute_goal->controller_type = 2;
+ execute_goal->spline_handle = 2;
+ execute_goal.Send();
+
+ WaitForTrajectoryExecution();
+ VerifyNearSplineGoal();
+ VerifyNearPosition(1.0, 2.0);
+}
+
// Tests that splines can excecute and plan at the same time.
TEST_F(DrivetrainTest, ParallelSplines) {
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 18c8381..2eb434a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -56,6 +56,8 @@
splines.emplace_back(Spline(points));
}
+ future_drive_spline_backwards_ = goal_.spline.drive_spline_backwards;
+
future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(::std::move(splines)));
@@ -93,18 +95,26 @@
void SplineDrivetrain::SetGoal(
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
current_spline_handle_ = goal.spline_handle;
- // If told to stop, set the executing spline to an invalid index.
- if (current_spline_handle_ == 0 && has_started_execution_) {
+ // If told to stop, set the executing spline to an invalid index and clear out
+ // its plan:
+ if (current_spline_handle_ == 0 &&
+ current_spline_idx_ != goal.spline.spline_idx) {
current_spline_idx_ = -1;
}
::aos::Mutex::State mutex_state = mutex_.TryLock();
if (mutex_state == ::aos::Mutex::State::kLocked) {
- drive_spline_backwards_ = goal_.drive_spline_backwards;
if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
goal_ = goal;
new_goal_.Broadcast();
+ if (current_spline_handle_ != current_spline_idx_) {
+ // If we aren't going to actively execute the current spline, evict it's
+ // plan.
+ past_trajectory_ = std::move(current_trajectory_);
+ past_distance_spline_ = std::move(current_distance_spline_);
+ }
}
+ // If you never started executing the previous spline, you're screwed.
if (future_trajectory_ &&
(!current_trajectory_ ||
current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) ||
@@ -116,6 +126,7 @@
// Move the computed data to be executed.
current_trajectory_ = std::move(future_trajectory_);
current_distance_spline_ = std::move(future_distance_spline_);
+ current_drive_spline_backwards_ = future_drive_spline_backwards_;
current_spline_idx_ = future_spline_idx_;
// Reset internal state to a trajectory start position.
@@ -145,7 +156,7 @@
::Eigen::Matrix<double, 2, 5> K =
current_trajectory_->KForState(state, dt_config_.dt, Q, R);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- if (drive_spline_backwards_) {
+ if (current_drive_spline_backwards_) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
goal_state(2, 0) += M_PI;
@@ -196,12 +207,15 @@
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
status->trajectory_logging.x = goal_state(0);
status->trajectory_logging.y = goal_state(1);
- status->trajectory_logging.theta = goal_state(2);
+ status->trajectory_logging.theta = ::aos::math::NormalizeAngle(
+ goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0));
status->trajectory_logging.left_velocity = goal_state(3);
status->trajectory_logging.right_velocity = goal_state(4);
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
+ status->trajectory_logging.is_executed =
+ (current_spline_idx_ != -1) && IsAtEnd();
status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 59175b6..98c4e74 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -71,10 +71,10 @@
int32_t current_spline_handle_ = 0; // Current spline told to excecute.
int32_t current_spline_idx_ = 0; // Current executing spline.
bool has_started_execution_ = false;
- bool drive_spline_backwards_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
+ bool current_drive_spline_backwards_ = false;
// State required to compute the next iteration's output.
::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
@@ -100,6 +100,7 @@
::std::unique_ptr<DistanceSpline> future_distance_spline_;
::std::unique_ptr<Trajectory> past_trajectory_;
::std::unique_ptr<Trajectory> future_trajectory_;
+ bool future_drive_spline_backwards_ = false;
int32_t future_spline_idx_ = 0; // Current spline being computed.
::std::atomic<int32_t> planning_spline_idx_{-1};
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 90a45f6..364836a 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -59,7 +59,7 @@
static constexpr double kElevatorClearIntakeHeight = 0.4;
// Angle constraints for the wrist when below kElevatorClearDownHeight
- static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.05;
+ static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.15;
static constexpr double kWristMinAngle = -M_PI / 2.0 - 0.25;
// Angles outside of which the intake is fully clear of the wrist.
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index d024d3f..766de6e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -78,6 +78,8 @@
const ButtonLocation kPanelHPIntakeBackward(5, 5);
const ButtonLocation kRelease(2, 4);
+// Reuse quickturn for the cancel button.
+const ButtonLocation kCancelAutoMode(2, 3);
const ButtonLocation kReleaseButtonBoard(3, 4);
const ButtonLocation kResetLocalizerLeftForwards(3, 10);
const ButtonLocation kResetLocalizerLeftBackwards(3, 9);
@@ -131,10 +133,9 @@
Reader(::aos::EventLoop *event_loop)
: ::aos::input::ActionJoystickInput(
event_loop,
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
- // Set teleop to immediately resume after auto ends for sandstorm mode.
- set_run_teleop_in_auto(true);
-
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig(),
+ {.run_teleop_in_auto = true,
+ .cancel_auto_button = kCancelAutoMode}) {
const uint16_t team = ::aos::network::GetTeamNumber();
superstructure_queue.goal.FetchLatest();
if (superstructure_queue.goal.get()) {