Merge "Make pistol grip 0 a mechanical pistol grip"
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/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 248f1ce..631f97d 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -170,6 +170,10 @@
throttle = ::std::max(-1.0, throttle / 0.7);
}
+ if (data.IsPressed(slow_down_)) {
+ throttle *= 0.5;
+ }
+
if (!data.GetControlBit(ControlBit::kEnabled)) {
high_gear_ = default_high_gear_;
}
@@ -242,6 +246,7 @@
const ButtonLocation TopButton(1, 1);
const ButtonLocation SecondButton(1, 2);
+ const ButtonLocation BottomButton(1, 4);
// Non-existant button for nops.
const ButtonLocation DummyButton(1, 10);
@@ -264,7 +269,8 @@
kWheelHigh, kWheelLow, kTriggerVelocityHigh, kTriggerVelocityLow,
kTriggerTorqueHigh, kTriggerTorqueLow, kTriggerHigh, kTriggerLow,
kWheelVelocityHigh, kWheelVelocityLow, kWheelTorqueHigh,
- kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow, kTurn1, kTurn2));
+ kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow, kTurn1, kTurn2,
+ BottomButton));
result->set_default_high_gear(default_high_gear);
return result;
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index f4b0212..ec52368 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -182,7 +182,8 @@
driver_station::ButtonLocation shift_high,
driver_station::ButtonLocation shift_low,
driver_station::ButtonLocation turn1,
- driver_station::ButtonLocation turn2)
+ driver_station::ButtonLocation turn2,
+ driver_station::ButtonLocation slow_down)
: DrivetrainInputReader(wheel_high, throttle_high, quick_turn, turn1,
TurnButtonUse::kLineFollow, turn2,
TurnButtonUse::kControlLoopDriving),
@@ -197,7 +198,8 @@
throttle_torque_high_(throttle_torque_high),
throttle_torque_low_(throttle_torque_low),
shift_high_(shift_high),
- shift_low_(shift_low) {}
+ shift_low_(shift_low),
+ slow_down_(slow_down) {}
WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) override;
@@ -222,6 +224,7 @@
const driver_station::ButtonLocation shift_high_;
const driver_station::ButtonLocation shift_low_;
+ const driver_station::ButtonLocation slow_down_;
bool high_gear_;
bool default_high_gear_;
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index 8aab97d..5d88d32 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -37,10 +37,7 @@
void ReadEvent() override { reader_->HandleFrame(); }
- bool SetExposure(int abs_exp) {
- return reader_->SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
- "V4L2_CID_EXPOSURE_ABSOLUTE", abs_exp);
- }
+ bool SetExposure(int abs_exp) { return reader_->SetExposure(abs_exp); }
private:
void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp);
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
index 23b58c2..991339b 100644
--- a/aos/vision/image/reader.cc
+++ b/aos/vision/image/reader.cc
@@ -56,6 +56,8 @@
Init();
InitMMap();
+
+ SetExposure(params.exposure());
LOG(INFO, "Bat Vision Successfully Initialized.\n");
}
@@ -168,6 +170,11 @@
return false;
}
+bool Reader::SetExposure(int abs_exp) {
+ return SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
+ "V4L2_CID_EXPOSURE_ABSOLUTE", abs_exp);
+}
+
void Reader::Init() {
v4l2_capability cap;
if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
diff --git a/aos/vision/image/reader.h b/aos/vision/image/reader.h
index 25fc0bd..4283224 100644
--- a/aos/vision/image/reader.h
+++ b/aos/vision/image/reader.h
@@ -33,6 +33,7 @@
int fd() { return fd_; }
bool SetCameraControl(uint32_t id, const char *name, int value);
+ bool SetExposure(int abs_exp);
private:
void QueueBuffer(v4l2_buffer *buf);
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index fc01edc..a71ee7d 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -350,6 +350,41 @@
}
}
+bool BaseAutonomousActor::SplineHandle::SplineDistanceRemaining(
+ double distance) {
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ return drivetrain_queue.status->trajectory_logging.is_executing &&
+ drivetrain_queue.status->trajectory_logging.distance_remaining <
+ distance;
+ }
+ return false;
+}
+bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
+ double distance) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (base_autonomous_actor_->ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ if (SplineDistanceRemaining(distance)) {
+ return true;
+ }
+ }
+}
+
+void BaseAutonomousActor::LineFollowAtVelocity(double velocity) {
+ auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ drivetrain_message->controller_type = 3;
+ // TODO(james): Currently the 4.0 is copied from the
+ // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
+ // factor it out in some way.
+ drivetrain_message->throttle = velocity / 4.0;
+ drivetrain_message.Send();
+}
+
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
const ::frc971::MultiSpline &spline, SplineDirection direction) {
LOG(INFO, "Planning spline\n");
@@ -364,7 +399,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 +456,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/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index aeef38c..e060a2c 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -29,7 +29,10 @@
bool IsDone();
bool WaitForDone();
- // Wait for done, wait until X from the end, wait for distance from the end
+ // Whether there is less than a certain distance, in meters, remaining in
+ // the current spline.
+ bool SplineDistanceRemaining(double distance);
+ bool WaitForSplineDistanceRemaining(double distance);
private:
friend BaseAutonomousActor;
@@ -67,6 +70,8 @@
// Returns true if the drive has finished.
bool IsDriveDone();
+ void LineFollowAtVelocity(double velocity);
+
// Waits until the robot is pitched up above the specified angle, or the move
// finishes. Returns true on success, and false if it cancels.
bool WaitForAboveAngle(double angle);
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.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2af6765..4e13a40 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -243,10 +243,11 @@
// simulation.
if (localizer_control_fetcher_.Fetch()) {
LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
- localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
- localizer_control_fetcher_->y,
- localizer_control_fetcher_->theta,
- localizer_control_fetcher_->theta_uncertainty);
+ localizer_->ResetPosition(
+ monotonic_now, localizer_control_fetcher_->x,
+ localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
+ localizer_control_fetcher_->theta_uncertainty,
+ !localizer_control_fetcher_->keep_current_theta);
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
monotonic_now, position->left_encoder,
@@ -330,7 +331,7 @@
status->x = localizer_->x();
status->y = localizer_->y();
- status->theta = localizer_->theta();
+ status->theta = ::aos::math::NormalizeAngle(localizer_->theta());
status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 09ecb4f..c6af732 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;
@@ -61,6 +63,7 @@
float theta;
float left_velocity;
float right_velocity;
+ float distance_remaining;
};
// For logging state of the line follower.
@@ -130,8 +133,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/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index f006bb4..ae92069 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -459,7 +459,7 @@
// since the wheels aren't likely to slip much stopped.
Q_continuous_(kX, kX) = 0.002;
Q_continuous_(kY, kY) = 0.002;
- Q_continuous_(kTheta, kTheta) = 0.0002;
+ Q_continuous_(kTheta, kTheta) = 0.0001;
Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.15, 2.0);
Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.15, 2.0);
Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.5, 2.0);
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index bd48315..23a978c 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -50,8 +50,8 @@
double gyro_rate, double longitudinal_accelerometer) = 0;
// Reset the absolute position of the estimator.
virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
- double y, double theta,
- double theta_uncertainty) = 0;
+ double y, double theta, double theta_uncertainty,
+ bool reset_theta) = 0;
// There are several subtly different norms floating around for state
// matrices. In order to avoid that mess, we jus tprovide direct accessors for
// the values that most people care about.
@@ -110,7 +110,8 @@
}
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta, double /*theta_override*/) override {
+ double theta, double /*theta_override*/,
+ bool /*reset_theta*/) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
index 8fef686..b1169a9 100644
--- a/frc971/control_loops/drivetrain/localizer.q
+++ b/frc971/control_loops/drivetrain/localizer.q
@@ -7,6 +7,7 @@
float y; // Y position, meters
float theta; // heading, radians
double theta_uncertainty; // Uncertainty in theta.
+ bool keep_current_theta; // Whether to keep the current theta value.
};
queue LocalizerControl localizer_control;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 18c8381..1f7e2b0 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,14 +207,20 @@
::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_;
+ status->trajectory_logging.distance_remaining =
+ current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
+ : 0.0;
int32_t planning_spline_idx = planning_spline_idx_;
if (current_spline_idx_ == planning_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/constants.cc b/y2019/constants.cc
index 8872318..477209e 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -145,7 +145,7 @@
r->camera_noise_parameters = {.max_viewable_distance = 10.0,
.heading_noise = 0.1,
.nominal_distance_noise = 0.15,
- .nominal_skew_noise = 0.45,
+ .nominal_skew_noise = 0.75,
.nominal_height_noise = 0.01};
// Deliberately make FOV a bit large so that we are overly conservative in
@@ -197,7 +197,7 @@
elevator_params->zeroing_constants.measured_absolute_position = 0.131806;
elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151 - 0.007513 + 0.007311;
- intake->zeroing_constants.measured_absolute_position = 1.756847;
+ intake->zeroing_constants.measured_absolute_position = 1.928755;
wrist_params->zeroing_constants.measured_absolute_position = 0.192576;
wrist->potentiometer_offset = -4.200894 - 0.187134;
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 282ca9e..889133f 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -34,7 +34,7 @@
target_selector_(event_loop) {
localizer_.ResetInitialState(::aos::monotonic_clock::now(),
Localizer::State::Zero(), localizer_.P());
- ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0);
+ ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0, true);
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 9255109..183e817 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -36,13 +36,16 @@
void Reset(::aos::monotonic_clock::time_point t,
const Localizer::State &state, double theta_uncertainty);
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta, double theta_uncertainty) override {
+ double theta, double theta_uncertainty,
+ bool reset_theta) override {
// When we reset the state, we want to keep the encoder positions intact, so
// we copy from the original state and reset everything else.
Localizer::State new_state = localizer_.X_hat();
new_state.x() = x;
new_state.y() = y;
- new_state(2, 0) = theta;
+ if (reset_theta) {
+ new_state(2, 0) = theta;
+ }
// Velocity terms.
new_state(4, 0) = 0.0;
new_state(6, 0) = 0.0;
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index d30ee4b..d008d54 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -478,7 +478,7 @@
GetParam().estimate_tolerance);
// Check that none of the states that we actually care about (x/y/theta, and
// wheel positions/speeds) are too far off, individually:
- EXPECT_LT(estimate_err.template topRows<7>().cwiseAbs().maxCoeff(),
+ EXPECT_LT(estimate_err.template topRows<3>().cwiseAbs().maxCoeff(),
GetParam().estimate_tolerance / 3.0)
<< "Estimate error: " << estimate_err.transpose();
Eigen::Matrix<double, 5, 1> final_trajectory_state;
@@ -597,7 +597,7 @@
.finished(),
/*noisify=*/true,
/*disturb=*/true,
- /*estimate_tolerance=*/0.15,
+ /*estimate_tolerance=*/0.2,
/*goal_tolerance=*/0.5,
}),
// Try another spline, just in case the one I was using is special for
@@ -614,6 +614,8 @@
.finished(),
/*noisify=*/true,
/*disturb=*/false,
+ // TODO(james): Improve tests so that we aren't constantly
+ // readjusting the tolerances up.
/*estimate_tolerance=*/0.3,
/*goal_tolerance=*/0.7,
})));
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 4c001ad..0086f5d 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -47,10 +47,10 @@
double TargetRadius() const override { return target_radius_; }
private:
- static constexpr double kFakeFov = M_PI * 0.7;
+ static constexpr double kFakeFov = M_PI * 0.9;
// Longitudinal speed at which the robot must be going in order for us to make
// a decision.
- static constexpr double kMinDecisionSpeed = 0.7; // m/s
+ static constexpr double kMinDecisionSpeed = 0.3; // m/s
Pose robot_pose_;
Pose target_pose_;
double target_radius_;
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/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index d5c8a73..f78bd5a 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -72,6 +72,12 @@
// Position of the stilts, 0 when retracted (defualt), positive lifts robot.
.frc971.PotAndAbsolutePosition stilts;
+
+ // True if the platform detection sensors detect the platform directly
+ // below the robot right behind the left and right wheels. Useful for
+ // determining when the robot is all the way on the platform.
+ bool platform_left_detect;
+ bool platform_right_detect;
};
message Output {
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 645582d..b3f5aa8 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -44,17 +44,17 @@
using google::protobuf::StringPrintf;
-const ButtonLocation kSuctionBall(3, 13);
-const ButtonLocation kSuctionHatch(3, 12);
-const ButtonLocation kDeployStilt(3, 8);
-const ButtonLocation kHalfStilt(3, 6);
-const ButtonLocation kFallOver(3, 9);
-
struct ElevatorWristPosition {
double elevator;
double wrist;
};
+const ButtonLocation kSuctionBall(4, 2);
+const ButtonLocation kSuctionHatch(3, 15);
+const ButtonLocation kDeployStilt(4, 1);
+const ButtonLocation kHalfStilt(4, 3);
+const ButtonLocation kFallOver(3, 16);
+
const ButtonLocation kRocketForwardLower(5, 1);
const ButtonLocation kRocketForwardMiddle(5, 2);
const ButtonLocation kRocketForwardUpper(5, 4);
@@ -78,17 +78,26 @@
const ButtonLocation kPanelHPIntakeBackward(5, 5);
const ButtonLocation kRelease(2, 4);
-const ButtonLocation kResetLocalizerLeftForwards(3, 14);
-const ButtonLocation kResetLocalizerLeftBackwards(4, 12);
+// 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);
-const ButtonLocation kResetLocalizerRightForwards(4, 1);
-const ButtonLocation kResetLocalizerRightBackwards(4, 11);
+const ButtonLocation kResetLocalizerRightForwards(3, 8);
+const ButtonLocation kResetLocalizerRightBackwards(3, 7);
-const ButtonLocation kNearCargoHint(3, 15);
-const ButtonLocation kMidCargoHint(3, 16);
-const ButtonLocation kFarCargoHint(4, 2);
+const ButtonLocation kResetLocalizerLeft(3, 11);
+const ButtonLocation kResetLocalizerRight(3, 13);
-const ButtonLocation kCameraLog(3, 7);
+const ButtonLocation kNearCargoHint(3, 3);
+const ButtonLocation kMidCargoHint(3, 5);
+const ButtonLocation kFarCargoHint(3, 6);
+
+const JoystickAxis kCargoSelectorY(5, 6);
+const JoystickAxis kCargoSelectorX(5, 5);
+
+const ButtonLocation kCameraLog(3, 14);
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};
@@ -130,10 +139,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()) {
@@ -157,7 +165,7 @@
superstructure_queue.status.FetchLatest();
if (!superstructure_queue.status.get() ||
!superstructure_queue.position.get()) {
- LOG(ERROR, "Got no superstructure status packet.\n");
+ LOG(ERROR, "Got no superstructure status or position packet.\n");
return;
}
@@ -176,13 +184,47 @@
} else if (data.IsPressed(kFarCargoHint)) {
target_hint->suggested_target = 3;
} else {
- target_hint->suggested_target = 0;
+ const double cargo_joy_y = data.GetAxis(kCargoSelectorY);
+ const double cargo_joy_x = data.GetAxis(kCargoSelectorX);
+ if (cargo_joy_y > 0.5) {
+ target_hint->suggested_target = 3;
+ } else if (cargo_joy_y < -0.5) {
+ target_hint->suggested_target = 1;
+ } else if (::std::abs(cargo_joy_x) > 0.5) {
+ target_hint->suggested_target = 2;
+ } else {
+ target_hint->suggested_target = 0;
+ }
}
if (!target_hint.Send()) {
LOG(ERROR, "Failed to send target selector hint.\n");
}
}
+ if (data.PosEdge(kResetLocalizerLeft)) {
+ auto localizer_resetter = localizer_control.MakeMessage();
+ // Start at the left feeder station.
+ localizer_resetter->x = 0.6;
+ localizer_resetter->y = 3.4;
+ localizer_resetter->keep_current_theta = true;
+
+ if (!localizer_resetter.Send()) {
+ LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
+
+ if (data.PosEdge(kResetLocalizerRight)) {
+ auto localizer_resetter = localizer_control.MakeMessage();
+ // Start at the left feeder station.
+ localizer_resetter->x = 0.6;
+ localizer_resetter->y = -3.4;
+ localizer_resetter->keep_current_theta = true;
+
+ if (!localizer_resetter.Send()) {
+ LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
+
if (data.PosEdge(kResetLocalizerLeftForwards)) {
auto localizer_resetter = localizer_control.MakeMessage();
// Start at the left feeder station.
@@ -231,11 +273,27 @@
}
}
+ if (data.PosEdge(kRelease) &&
+ monotonic_now >
+ last_release_button_press_ + ::std::chrono::milliseconds(500)) {
+ if (superstructure_queue.status->has_piece) {
+ release_mode_ = ReleaseButtonMode::kRelease;
+ } else {
+ release_mode_ = ReleaseButtonMode::kBallIntake;
+ }
+ }
+
+ if (data.IsPressed(kRelease)) {
+ last_release_button_press_ = monotonic_now;
+ }
+
if (data.IsPressed(kSuctionBall)) {
grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
grab_piece_ = true;
- } else if (data.IsPressed(kRelease) ||
+ } else if ((release_mode_ == ReleaseButtonMode::kRelease &&
+ data.IsPressed(kRelease)) ||
+ data.IsPressed(kReleaseButtonBoard) ||
!superstructure_queue.status->has_piece) {
grab_piece_ = false;
}
@@ -246,7 +304,10 @@
new_superstructure_goal->intake.unsafe_goal = -1.2;
new_superstructure_goal->roller_voltage = 0.0;
- const bool kDoBallIntake = data.GetAxis(kBallIntake) > 0.9;
+ const bool kDoBallIntake =
+ (!climbed_ && release_mode_ == ReleaseButtonMode::kBallIntake &&
+ data.IsPressed(kRelease)) ||
+ data.GetAxis(kBallIntake) > 0.9;
const bool kDoBallOutake = data.GetAxis(kBallOutake) > 0.9;
if (data.IsPressed(kPanelSwitch)) {
@@ -340,18 +401,18 @@
constexpr double kDeployStiltPosition = 0.5;
- // TODO(sabina): max height please?
if (data.IsPressed(kFallOver)) {
new_superstructure_goal->stilts.unsafe_goal = 0.71;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
new_superstructure_goal->stilts.profile_params.max_acceleration = 1.25;
- } else if (data.IsPressed(kDeployStilt)) {
+ } else if (data.IsPressed(kHalfStilt)) {
+ was_above_ = false;
+ new_superstructure_goal->stilts.unsafe_goal = 0.345;
+ new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+ } else if (data.IsPressed(kDeployStilt) || was_above_) {
new_superstructure_goal->stilts.unsafe_goal = kDeployStiltPosition;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
- } else if (data.IsPressed(kHalfStilt)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.345;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
} else {
new_superstructure_goal->stilts.unsafe_goal = 0.005;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
@@ -360,18 +421,32 @@
if (superstructure_queue.status->stilts.position > 0.1) {
elevator_wrist_pos_ = kClimbPos;
+ climbed_ = true;
new_superstructure_goal->wrist.profile_params.max_acceleration = 10;
new_superstructure_goal->elevator.profile_params.max_acceleration = 6;
}
+ // If we've been asked to go above deploy and made it up that high, latch
+ // was_above.
+ if (new_superstructure_goal->stilts.unsafe_goal > kDeployStiltPosition &&
+ superstructure_queue.status->stilts.position >= kDeployStiltPosition) {
+ was_above_ = true;
+ } else if ((superstructure_queue.position->platform_left_detect ||
+ superstructure_queue.position->platform_right_detect) &&
+ !data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
+ // TODO(austin): Should make this && rather than ||
+ was_above_ = false;
+ }
+
if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
}
-
- if (data.IsPressed(kRelease)) {
+ if ((release_mode_ == ReleaseButtonMode::kRelease &&
+ data.IsPressed(kRelease)) ||
+ data.IsPressed(kReleaseButtonBoard)) {
grab_piece_ = false;
}
@@ -418,12 +493,27 @@
return ::frc971::autonomous::auto_mode->mode;
}
+ // Bool to track if we've been above the deploy position. Once this bool is
+ // set, don't let the stilts retract until we see the platform.
+ bool was_above_ = false;
+
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
bool grab_piece_ = false;
bool switch_ball_ = false;
+ bool climbed_ = false;
+
+ enum class ReleaseButtonMode {
+ kBallIntake,
+ kRelease,
+ };
+
+ ReleaseButtonMode release_mode_ = ReleaseButtonMode::kRelease;
+ aos::monotonic_clock::time_point last_release_button_press_ =
+ aos::monotonic_clock::min_time;
+
VisionControl vision_control_;
::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
::aos::monotonic_clock::time_point last_vision_control_ =
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 6f2e26a..99bfc7c 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -198,8 +198,8 @@
LocalCameraFrame cur_frame = latest_frames[ii];
constants::Values::CameraCalibration camera_info =
constants::GetValues().cameras[ii];
- frc971::control_loops::TypedPose<double> camera_pose =
- camera_info.pose.Rebase(&robot_pose);
+ frc971::control_loops::TypedPose<double> camera_pose = camera_info.pose;
+ camera_pose.set_base(&robot_pose);
camera_debug->set_current_frame_age(
::std::chrono::duration_cast<::std::chrono::duration<double>>(
diff --git a/y2019/vision/server/www/robot.ts b/y2019/vision/server/www/robot.ts
index 71defb6..a106d5f 100644
--- a/y2019/vision/server/www/robot.ts
+++ b/y2019/vision/server/www/robot.ts
@@ -63,12 +63,15 @@
ctx.restore();
+ ctx.save();
+ ctx.lineWidth = 3.0 * ctx.lineWidth;
+ ctx.strokeStyle = 'yellow';
for (let frame of cameraFrames) {
if (frame.targets) {
for (let target of frame.targets) {
- ctx.strokeStyle = 'yellow';
drawTarget(ctx, target.x, target.y, target.theta);
}
}
}
+ ctx.restore();
}
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 95ed28e..17dabde 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -198,6 +198,16 @@
stilts_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ void set_platform_left_detect(
+ ::std::unique_ptr<frc::DigitalInput> platform_left_detect) {
+ platform_left_detect_ = ::std::move(platform_left_detect);
+ }
+
+ void set_platform_right_detect(
+ ::std::unique_ptr<frc::DigitalInput> platform_right_detect) {
+ platform_right_detect_ = ::std::move(platform_right_detect);
+ }
+
// Vacuum pressure sensor
void set_vacuum_sensor(int port) {
vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
@@ -257,6 +267,11 @@
(vacuum_sensor_->GetVoltage() - kMinVoltage) /
(kMaxVoltage - kMinVoltage);
+ superstructure_message->platform_left_detect =
+ !platform_left_detect_->Get();
+ superstructure_message->platform_right_detect =
+ !platform_right_detect_->Get();
+
superstructure_message.Send();
}
@@ -277,6 +292,9 @@
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;
+ ::std::unique_ptr<frc::DigitalInput> platform_left_detect_;
+ ::std::unique_ptr<frc::DigitalInput> platform_right_detect_;
+
::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
@@ -668,6 +686,9 @@
reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
+ reader.set_platform_right_detect(make_unique<frc::DigitalInput>(6));
+ reader.set_platform_left_detect(make_unique<frc::DigitalInput>(7));
+
reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));