Merge changes I7f3c1cc5,I9744c480,Id8ac2fae
* changes:
Make target selector use throttle instead of velocity
Tune localizer, remove error states
Add button to set line following mode
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 67ee9f3..02957a8 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -23,8 +23,6 @@
void DrivetrainInputReader::HandleDrivetrain(
const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
-
const auto wheel_and_throttle = GetWheelAndThrottle(data);
const double wheel = wheel_and_throttle.wheel;
const double wheel_velocity = wheel_and_throttle.wheel_velocity;
@@ -39,7 +37,32 @@
robot_velocity_ = drivetrain_queue.status->robot_speed;
}
- if (data.PosEdge(turn1_) || data.PosEdge(turn2_)) {
+ bool is_control_loop_driving = false;
+ bool is_line_following = false;
+
+ if (data.IsPressed(turn1_)) {
+ switch (turn1_use_) {
+ case TurnButtonUse::kControlLoopDriving:
+ is_control_loop_driving = true;
+ break;
+ case TurnButtonUse::kLineFollow:
+ is_line_following = true;
+ break;
+ }
+ }
+
+ if (data.IsPressed(turn2_)) {
+ switch (turn2_use_) {
+ case TurnButtonUse::kControlLoopDriving:
+ is_control_loop_driving = true;
+ break;
+ case TurnButtonUse::kLineFollow:
+ is_line_following = true;
+ break;
+ }
+ }
+
+ if (is_control_loop_driving) {
if (drivetrain_queue.status.get()) {
left_goal_ = drivetrain_queue.status->estimated_left_position;
right_goal_ = drivetrain_queue.status->estimated_right_position;
@@ -49,9 +72,6 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- if (data.IsPressed(turn1_) || data.IsPressed(turn2_)) {
- is_control_loop_driving = true;
- }
auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
new_drivetrain_goal->wheel = wheel;
new_drivetrain_goal->wheel_velocity = wheel_velocity;
@@ -61,7 +81,8 @@
new_drivetrain_goal->throttle_torque = throttle_torque;
new_drivetrain_goal->highgear = high_gear;
new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
- new_drivetrain_goal->controller_type = is_control_loop_driving ? 1 : 0;
+ new_drivetrain_goal->controller_type =
+ is_line_following ? 3 : (is_control_loop_driving ? 1 : 0);
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
@@ -192,15 +213,17 @@
const ButtonLocation kTurn1(1, 7);
const ButtonLocation kTurn2(1, 11);
std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
- new SteeringWheelDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
- kQuickTurn, kTurn1, kTurn2));
+ new SteeringWheelDrivetrainInputReader(
+ kSteeringWheel, kDriveThrottle, kQuickTurn, kTurn1,
+ TurnButtonUse::kControlLoopDriving, kTurn2,
+ TurnButtonUse::kControlLoopDriving));
result.get()->set_default_high_gear(default_high_gear);
return result;
}
std::unique_ptr<PistolDrivetrainInputReader> PistolDrivetrainInputReader::Make(
- bool default_high_gear) {
+ bool default_high_gear, TopButtonUse top_button_use) {
// Pistol Grip controller
const JoystickAxis kTriggerHigh(1, 1), kTriggerLow(1, 4),
kTriggerVelocityHigh(1, 2), kTriggerVelocityLow(1, 5),
@@ -211,12 +234,23 @@
kWheelTorqueLow(2, 6);
const ButtonLocation kQuickTurn(1, 3);
- const ButtonLocation kShiftHigh(1, 1);
- const ButtonLocation kShiftLow(1, 2);
- // Nop
- const ButtonLocation kTurn1(1, 9);
- const ButtonLocation kTurn2(1, 10);
+ const ButtonLocation TopButton(1, 1);
+ const ButtonLocation SecondButton(1, 2);
+ // Non-existant button for nops.
+ const ButtonLocation DummyButton(1, 10);
+
+ // TODO(james): Make a copy assignment operator for ButtonLocation so we don't
+ // have to shoehorn in these ternary operators.
+ const ButtonLocation kTurn1 =
+ (top_button_use == TopButtonUse::kLineFollow) ? TopButton : DummyButton;
+ // Turn2 currently does nothing on the pistol grip, ever.
+ const ButtonLocation kTurn2 = DummyButton;
+ const ButtonLocation kShiftHigh =
+ (top_button_use == TopButtonUse::kShift) ? TopButton : DummyButton;
+ const ButtonLocation kShiftLow =
+ (top_button_use == TopButtonUse::kShift) ? SecondButton : DummyButton;
+
std::unique_ptr<PistolDrivetrainInputReader> result(
new PistolDrivetrainInputReader(
kWheelHigh, kWheelLow, kTriggerVelocityHigh, kTriggerVelocityLow,
@@ -239,7 +273,9 @@
std::unique_ptr<XboxDrivetrainInputReader> result(
new XboxDrivetrainInputReader(kSteeringWheel, kDriveThrottle, kQuickTurn,
- kTurn1, kTurn2));
+ kTurn1, TurnButtonUse::kControlLoopDriving,
+ kTurn2,
+ TurnButtonUse::kControlLoopDriving));
return result;
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
@@ -256,8 +292,11 @@
SteeringWheelDrivetrainInputReader::Make(dt_config.default_high_gear);
break;
case InputType::kPistol:
- drivetrain_input_reader =
- PistolDrivetrainInputReader::Make(dt_config.default_high_gear);
+ drivetrain_input_reader = PistolDrivetrainInputReader::Make(
+ dt_config.default_high_gear,
+ dt_config.pistol_grip_shift_enables_line_follow
+ ? PistolDrivetrainInputReader::TopButtonUse::kLineFollow
+ : PistolDrivetrainInputReader::TopButtonUse::kShift);
break;
case InputType::kXbox:
drivetrain_input_reader = XboxDrivetrainInputReader::Make();
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 5274cfa..80046ee 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -35,17 +35,28 @@
// joystick types.
class DrivetrainInputReader {
public:
+ // What to use the turn1/2 buttons for.
+ enum class TurnButtonUse {
+ // Use the button to enable control loop driving.
+ kControlLoopDriving,
+ // Use the button to set line following mode.
+ kLineFollow,
+ };
// Inputs driver station button and joystick locations
DrivetrainInputReader(driver_station::JoystickAxis wheel,
driver_station::JoystickAxis throttle,
driver_station::ButtonLocation quick_turn,
driver_station::ButtonLocation turn1,
- driver_station::ButtonLocation turn2)
+ TurnButtonUse turn1_use,
+ driver_station::ButtonLocation turn2,
+ TurnButtonUse turn2_use)
: wheel_(wheel),
throttle_(throttle),
quick_turn_(quick_turn),
turn1_(turn1),
- turn2_(turn2) {}
+ turn1_use_(turn1_use),
+ turn2_(turn2),
+ turn2_use_(turn2_use) {}
virtual ~DrivetrainInputReader() = default;
@@ -78,8 +89,12 @@
const driver_station::JoystickAxis wheel_;
const driver_station::JoystickAxis throttle_;
const driver_station::ButtonLocation quick_turn_;
+ // Button for enabling control loop driving.
const driver_station::ButtonLocation turn1_;
+ const TurnButtonUse turn1_use_;
+ // But for enabling line following.
const driver_station::ButtonLocation turn2_;
+ const TurnButtonUse turn2_use_;
// Structure containing the (potentially adjusted) steering and throttle
// values from the joysticks.
@@ -134,9 +149,18 @@
public:
using DrivetrainInputReader::DrivetrainInputReader;
+ // What to use the top two buttons for on the pistol grip.
+ enum class TopButtonUse {
+ // Normal shifting.
+ kShift,
+ // Line following (currently just uses top button).
+ kLineFollow,
+ };
+
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the (cheap) pistol grip controller.
- static std::unique_ptr<PistolDrivetrainInputReader> Make(bool default_high_gear);
+ static std::unique_ptr<PistolDrivetrainInputReader> Make(
+ bool default_high_gear, TopButtonUse top_button_use);
private:
PistolDrivetrainInputReader(
@@ -158,7 +182,8 @@
driver_station::ButtonLocation turn1,
driver_station::ButtonLocation turn2)
: DrivetrainInputReader(wheel_high, throttle_high, quick_turn, turn1,
- turn2),
+ TurnButtonUse::kLineFollow, turn2,
+ TurnButtonUse::kLineFollow),
wheel_low_(wheel_low),
wheel_velocity_high_(wheel_velocity_high),
wheel_velocity_low_(wheel_velocity_low),
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index a4d2ca6..d3ed728 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -46,6 +46,7 @@
deps = [
":drivetrain_config",
"//aos/containers:priority_queue",
+ "//aos/util:math",
"//frc971/control_loops:c2d",
"//frc971/control_loops:runge_kutta",
"//third_party/eigen",
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 0f57bf9..53c2315 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -92,6 +92,9 @@
Scalar wheel_multiplier;
+ // Whether the shift button on the pistol grip enables line following mode.
+ bool pistol_grip_shift_enables_line_follow = false;
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 82f409c..119386a 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -4,6 +4,7 @@
#include <chrono>
#include "aos/containers/priority_queue.h"
+#include "aos/util/math.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/runge_kutta.h"
#include "Eigen/Dense"
@@ -48,11 +49,8 @@
kLeftVelocity = 4,
kRightEncoder = 5,
kRightVelocity = 6,
- kLeftVoltageError = 7,
- kRightVoltageError = 8 ,
- kAngularError = 9,
};
- static constexpr int kNStates = 10;
+ static constexpr int kNStates = 7;
static constexpr int kNInputs = 2;
// Number of previous samples to save.
static constexpr int kSaveSamples = 50;
@@ -70,19 +68,11 @@
// variable-size measurement updates.
typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output;
typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare;
- // State is [x_position, y_position, theta, Kalman States], where
- // Kalman States are the states from the standard drivetrain Kalman Filter,
- // which is: [left encoder, left ground vel, right encoder, right ground vel,
- // left voltage error, right voltage error, angular_error], where:
- // left/right encoder should correspond directly to encoder readings
- // left/right velocities are the velocity of the left/right sides over the
+ // State is [x_position, y_position, theta, left encoder, left ground vel,
+ // right encoder, right ground vel]. left/right encoder should correspond
+ // directly to encoder readings left/right velocities are the velocity of the
+ // left/right sides over the
// ground (i.e., corrected for angular_error).
- // voltage errors are the difference between commanded and effective voltage,
- // used to estimate consistent modelling errors (e.g., friction).
- // angular error is the difference between the angular velocity as estimated
- // by the encoders vs. estimated by the gyro, such as might be caused by
- // wheels on one side of the drivetrain being too small or one side's
- // wheels slipping more than the other.
typedef Eigen::Matrix<Scalar, kNStates, 1> State;
// Constructs a HybridEkf for a particular drivetrain.
@@ -406,9 +396,7 @@
// Encoder derivatives
A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
- A_continuous_(kLeftEncoder, kAngularError) = 1.0;
A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
- A_continuous_(kRightEncoder, kAngularError) = -1.0;
// Pull velocity derivatives from velocity matrices.
// Note that this looks really awkward (doesn't use
@@ -425,21 +413,22 @@
B_continuous_.setZero();
B_continuous_.row(kLeftVelocity) = vel_coefs.B_continuous.row(0);
B_continuous_.row(kRightVelocity) = vel_coefs.B_continuous.row(1);
- A_continuous_.template block<kNStates, kNInputs>(0, 7) = B_continuous_;
Q_continuous_.setZero();
// TODO(james): Improve estimates of process noise--e.g., X/Y noise can
// probably be reduced when we are stopped because you rarely jump randomly.
// Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed,
// since the wheels aren't likely to slip much stopped.
- Q_continuous_(kX, kX) = 0.005;
- Q_continuous_(kY, kY) = 0.005;
- Q_continuous_(kTheta, kTheta) = 0.001;
- Q_continuous_.template block<7, 7>(3, 3) =
- dt_config_.make_kf_drivetrain_loop().observer().coefficients().Q;
+ Q_continuous_(kX, kX) = 0.01;
+ Q_continuous_(kY, kY) = 0.01;
+ Q_continuous_(kTheta, kTheta) = 0.0002;
+ Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.03, 2.0);
+ Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.03, 2.0);
+ Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.1, 2.0);
+ Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.1, 2.0);
P_.setZero();
- P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
+ P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01;
H_encoders_and_gyro_.setZero();
// Encoders are stored directly in the state matrix, so are a minor
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 27119b1..1702ec4 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -52,22 +52,16 @@
EXPECT_EQ(Xdot_ekf(StateIdx::kX, 0), ctheta * (left_vel + right_vel) / 2.0);
EXPECT_EQ(Xdot_ekf(StateIdx::kY, 0), stheta * (left_vel + right_vel) / 2.0);
EXPECT_EQ(Xdot_ekf(StateIdx::kTheta, 0), (right_vel - left_vel) / diameter);
- EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0),
- left_vel + X(StateIdx::kAngularError, 0));
- EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0),
- right_vel - X(StateIdx::kAngularError, 0));
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0), left_vel);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0), right_vel);
Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity, 0),
X(StateIdx::kRightVelocity, 0));
Eigen::Matrix<double, 2, 1> expected_vel_X =
velocity_plant_coefs_.A_continuous * vel_x +
- velocity_plant_coefs_.B_continuous *
- (U + X.middleRows<2>(StateIdx::kLeftVoltageError));
+ velocity_plant_coefs_.B_continuous * U;
EXPECT_EQ(Xdot_ekf(StateIdx::kLeftVelocity, 0), expected_vel_X(0, 0));
EXPECT_EQ(Xdot_ekf(StateIdx::kRightVelocity, 0), expected_vel_X(1, 0));
-
- // Dynamics don't expect error terms to change:
- EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
}
State DiffEq(const State &X, const Input &U) {
return ekf_.DiffEq(X, U);
@@ -93,18 +87,14 @@
CheckDiffEq(State::Zero(), Input::Zero());
CheckDiffEq(State::Zero(), {-5.0, 5.0});
CheckDiffEq(State::Zero(), {12.0, -3.0});
- CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6).finished(),
{5.0, 6.0});
- CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6).finished(),
{5.0, 6.0});
- CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6).finished(),
{5.0, 6.0});
// And check that a theta outisde of [-M_PI, M_PI] works.
- CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6).finished(),
{5.0, 6.0});
}
@@ -112,7 +102,7 @@
// with zero change in time, the state should approach the estimation.
TEST_F(HybridEkfTest, ZeroTimeCorrect) {
HybridEkf<>::Output Z(0.5, 0.5, 1);
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 7> H;
H.setIdentity();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
@@ -140,7 +130,7 @@
HybridEkf<>::Output Z(0, 0, 0);
// Use true_X to track what we think the true robot state is.
State true_X = ekf_.X_hat();
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 7> H;
H.setZero();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
@@ -171,9 +161,6 @@
EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftVelocity) * 0.8,
ekf_.X_hat(StateIdx::kRightVelocity),
ekf_.X_hat(StateIdx::kLeftVelocity) * 0.1);
- EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLeftVoltageError));
- EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kRightVoltageError));
- EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kAngularError));
const double ending_p_norm = ekf_.P().norm();
// Due to lack of corrections, noise should've increased.
EXPECT_GT(ending_p_norm, starting_p_norm * 1.10);
@@ -193,7 +180,7 @@
TEST_P(HybridEkfOldCorrectionsTest, CreateOldCorrection) {
HybridEkf<>::Output Z;
Z.setZero();
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 7> H;
H.setZero();
auto h_zero = [H](const State &X, const Input &) { return H * X; };
auto dhdx_zero = [H](const State &) { return H; };
@@ -231,7 +218,7 @@
expected_X_hat(0, 0) = Z(0, 0);
expected_X_hat(1, 0) = Z(1, 0) + modeled_X_hat(0, 0);
expected_X_hat(2, 0) = Z(2, 0);
- EXPECT_LT((expected_X_hat.topRows<7>() - ekf_.X_hat().topRows<7>()).norm(),
+ EXPECT_LT((expected_X_hat - ekf_.X_hat()).norm(),
1e-3)
<< "X_hat: " << ekf_.X_hat() << " expected " << expected_X_hat;
// The covariance after the predictions but before the corrections should
@@ -249,7 +236,7 @@
TEST_F(HybridEkfTest, DiscardTooOldCorrection) {
HybridEkf<>::Output Z;
Z.setZero();
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 7> H;
H.setZero();
auto h_zero = [H](const State &X, const Input &) { return H * X; };
auto dhdx_zero = [H](const State &) { return H; };
@@ -304,11 +291,11 @@
}
// Tests that encoder updates cause everything to converge properly in the
-// presence of voltage error.
+// presence of an initial velocity error.
TEST_F(HybridEkfTest, PerfectEncoderUpdatesWithVoltageError) {
State true_X = ekf_.X_hat();
- true_X(StateIdx::kLeftVoltageError, 0) = 2.0;
- true_X(StateIdx::kRightVoltageError, 0) = 2.0;
+ true_X(StateIdx::kLeftVelocity, 0) = 0.2;
+ true_X(StateIdx::kRightVelocity, 0) = 0.2;
Input U(5.0, 5.0);
for (int ii = 0; ii < 1000; ++ii) {
true_X = Update(true_X, U);
@@ -328,11 +315,11 @@
// Tests encoder/gyro updates when we have some errors in our estimate.
TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
- // In order to simulate modelling errors, we add an angular_error and start
- // the encoder values slightly off.
+ // In order to simulate modelling errors, we start the encoder values slightly
+ // off.
State true_X = ekf_.X_hat();
- true_X(StateIdx::kAngularError, 0) = 1.0;
true_X(StateIdx::kLeftEncoder, 0) += 2.0;
+ true_X(StateIdx::kLeftVelocity, 0) = 0.1;
true_X(StateIdx::kRightEncoder, 0) -= 2.0;
// After enough time, everything should converge to near-perfect (if there
// were any errors in the original absolute state (x/y/theta) state, then we
@@ -350,7 +337,7 @@
dt_config_.robot_radius / 2.0,
U, t0_ + (ii + 1) * dt_config_.dt);
}
- EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 1e-5)
+ EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 1e-4)
<< "Expected non-x/y estimates to converge to correct. "
"Estimated X_hat:\n"
<< ekf_.X_hat() << "\ntrue X:\n"
@@ -359,11 +346,11 @@
// Tests encoder/gyro updates in a realistic-ish scenario with noise:
TEST_F(HybridEkfTest, RealisticEncoderUpdateConverges) {
- // In order to simulate modelling errors, we add an angular_error and start
- // the encoder values slightly off.
+ // In order to simulate modelling errors, we start the encoder values slightly
+ // off.
State true_X = ekf_.X_hat();
- true_X(StateIdx::kAngularError, 0) = 1.0;
true_X(StateIdx::kLeftEncoder, 0) += 2.0;
+ true_X(StateIdx::kLeftVelocity, 0) = 0.1;
true_X(StateIdx::kRightEncoder, 0) -= 2.0;
Input U(10.0, 5.0);
for (int ii = 0; ii < 100; ++ii) {
@@ -377,7 +364,7 @@
U, t0_ + (ii + 1) * dt_config_.dt);
}
EXPECT_NEAR(
- (true_X.bottomRows<9>() - ekf_.X_hat().bottomRows<9>()).squaredNorm(),
+ (true_X.bottomRows<6>() - ekf_.X_hat().bottomRows<6>()).squaredNorm(),
0.0, 2e-3)
<< "Expected non-x/y estimates to converge to correct. "
"Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
@@ -411,7 +398,7 @@
// Check that we die when only one of h and dhdx are provided:
EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {},
[](const State &) {
- return Eigen::Matrix<double, 3, 10>::Zero();
+ return Eigen::Matrix<double, 3, 7>::Zero();
},
{}, t0_ + ::std::chrono::seconds(1)),
"make_h");
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index d085d17..b5417af 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -182,7 +182,8 @@
// Because we assume the target selector may have some internal state (e.g.,
// not confirming a target until some time as passed), we should call
// UpdateSelection every time.
- bool new_target = target_selector_->UpdateSelection(abs_state);
+ bool new_target =
+ target_selector_->UpdateSelection(abs_state, goal_velocity_);
if (freeze_target_) {
// When freezing the target, only make changes if we didn't have a good
// target before.
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index af07089..f0685dd 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -16,9 +16,12 @@
// Take the state as [x, y, theta, left_vel, right_vel]
// If unable to determine what target to go for, returns false. If a viable
// target is selected, then returns true and sets target_pose.
+ // command_speed is the goal speed of the current drivetrain, generally
+ // generated from the throttle and meant to signify driver intent.
// TODO(james): Some implementations may also want a drivetrain goal so that
// driver intent can be divined more directly.
- virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) = 0;
+ virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) = 0;
// Gets the current target pose. Should only be called if UpdateSelection has
// returned true.
virtual TypedPose<double> TargetPose() const = 0;
@@ -59,7 +62,7 @@
// manually set the target selector state.
class TrivialTargetSelector : public TargetSelectorInterface {
public:
- bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &) override {
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &, double) override {
return has_target_;
}
TypedPose<double> TargetPose() const override { return pose_; }
@@ -95,9 +98,12 @@
}
void ResetPosition(double x, double y, double theta) override {
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(
::aos::monotonic_clock::now(),
- (Ekf::State() << x, y, theta, 0, 0, 0, 0, 0, 0, 0).finished(),
+ (Ekf::State() << x, y, theta, left_encoder, 0, right_encoder, 0)
+ .finished(),
ekf_.P());
};
@@ -110,12 +116,8 @@
double right_velocity() const override {
return ekf_.X_hat(StateIdx::kRightVelocity);
}
- double left_voltage_error() const override {
- return ekf_.X_hat(StateIdx::kLeftVoltageError);
- }
- double right_voltage_error() const override {
- return ekf_.X_hat(StateIdx::kRightVoltageError);
- }
+ double left_voltage_error() const override { return 0.0; }
+ double right_voltage_error() const override { return 0.0; }
TrivialTargetSelector *target_selector() override {
return &target_selector_;
diff --git a/y2019/constants.cc b/y2019/constants.cc
index c6cc023..285cb21 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -143,9 +143,9 @@
stilts_params->zeroing_constants.allowable_encoder_error = 0.9;
r->camera_noise_parameters = {.max_viewable_distance = 10.0,
- .heading_noise = 0.02,
- .nominal_distance_noise = 0.06,
- .nominal_skew_noise = 0.1,
+ .heading_noise = 0.2,
+ .nominal_distance_noise = 0.3,
+ .nominal_skew_noise = 0.35,
.nominal_height_noise = 0.01};
// Deliberately make FOV a bit large so that we are overly conservative in
@@ -201,6 +201,8 @@
stilts_params->zeroing_constants.measured_absolute_position = 0.043580;
stilts->potentiometer_offset = -0.093820 + 0.0124 - 0.008334 + 0.004507;
+
+ FillCameraPoses(vision::PracticeBotTeensyId(), &r->cameras);
break;
case kCodingRobotTeamNumber:
@@ -306,8 +308,8 @@
constexpr double kHpSlotY = InchToMeters((26 * 12 + 10.5) / 2.0 - 25.9);
constexpr double kHpSlotTheta = M_PI;
- constexpr double kNormalZ = 0.80;
- constexpr double kPortZ = 0.99;
+ constexpr double kNormalZ = 0.85;
+ constexpr double kPortZ = 1.04;
const Pose far_side_cargo_bay({kFarSideCargoBayX, kSideCargoBayY, kNormalZ},
kSideCargoBayTheta);
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.cc b/y2019/control_loops/drivetrain/drivetrain_base.cc
index 02bd805..1dfe99a 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_base.cc
@@ -28,20 +28,31 @@
::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
::frc971::control_loops::drivetrain::IMUType::IMU_X,
- drivetrain::MakeDrivetrainLoop, drivetrain::MakeVelocityDrivetrainLoop,
+ drivetrain::MakeDrivetrainLoop,
+ drivetrain::MakeVelocityDrivetrainLoop,
drivetrain::MakeKFDrivetrainLoop,
drivetrain::MakeHybridVelocityDrivetrainLoop,
chrono::duration_cast<chrono::nanoseconds>(
chrono::duration<double>(drivetrain::kDt)),
- drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
- drivetrain::kHighGearRatio, drivetrain::kLowGearRatio, drivetrain::kJ,
- drivetrain::kMass, kThreeStateDriveShifter, kThreeStateDriveShifter,
- true /* default_high_gear */, 0 /* down_offset if using constants use
- constants::GetValues().down_error */,
- 0.7 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
+ drivetrain::kHighGearRatio,
+ drivetrain::kLowGearRatio,
+ drivetrain::kJ,
+ drivetrain::kMass,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true /* default_high_gear */,
+ 0 /* down_offset if using constants use
+ constants::GetValues().down_error */
+ ,
+ 0.7 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */,
1.2 /* wheel_multiplier */,
+ true /*pistol_grip_shift_enables_line_follow*/,
};
return kDrivetrainConfig;
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index aaf377c..9207ec1 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -35,15 +35,15 @@
void Reset(const Localizer::State &state);
void ResetPosition(double x, double y, double 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;
+ // Velocity terms.
new_state(4, 0) = 0.0;
new_state(6, 0) = 0.0;
- new_state(7, 0) = 0.0;
- new_state(8, 0) = 0.0;
- new_state(9, 0) = 0.0;
Reset(new_state);
}
@@ -64,12 +64,8 @@
double right_velocity() const override {
return localizer_.X_hat(StateIdx::kRightVelocity);
}
- double left_voltage_error() const override {
- return localizer_.X_hat(StateIdx::kLeftVoltageError);
- }
- double right_voltage_error() const override {
- return localizer_.X_hat(StateIdx::kRightVoltageError);
- }
+ double left_voltage_error() const override { return 0.0; }
+ double right_voltage_error() const override { return 0.0; }
TargetSelector *target_selector() override {
return &target_selector_;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 8a65170..e086e36 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -67,7 +67,7 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_motor_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<double, 10, 1> localizer_state;
+ Eigen::Matrix<double, 7, 1> localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
localizer_.Reset(localizer_state);
@@ -296,7 +296,7 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(1e-5);
+ VerifyEstimatorAccurate(1e-4);
}
namespace {
@@ -310,11 +310,11 @@
SetStartingPosition({4, 3, M_PI});
my_drivetrain_queue_.goal.MakeWithBuilder()
.controller_type(3)
- .throttle(0.5)
+ .throttle(0.9)
.Send();
RunForTime(chrono::seconds(10));
- VerifyEstimatorAccurate(1e-10);
+ VerifyEstimatorAccurate(1e-8);
// Due to the fact that we aren't modulating the throttle, we don't try to hit
// the target exactly. Instead, just run slightly past the target:
EXPECT_LT(::std::abs(::aos::math::DiffAngle(
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 8b6f7ed..f062234 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -399,7 +399,7 @@
U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);
state = ::frc971::control_loops::RungeKuttaU(
- [this](const ::Eigen::Matrix<double, 10, 1> &X,
+ [this](const ::Eigen::Matrix<double, 7, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
state, U,
::std::chrono::duration_cast<::std::chrono::duration<double>>(
@@ -418,7 +418,7 @@
::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
3.0);
TestLocalizer::State disturbance;
- disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0;
+ disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0;
disturbance *= disturbance_scale;
state += disturbance;
}
@@ -498,11 +498,9 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
@@ -514,11 +512,9 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
@@ -529,11 +525,9 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
@@ -544,26 +538,9 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0)
- .finished(),
- /*noisify=*/false,
- /*disturb=*/false,
- /*estimate_tolerance=*/1e-4,
- /*goal_tolerance=*/2e-2,
- }),
- // Repeats perfect scenario, but add voltage + angular errors:
- LocalizerTestParams({
- /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
- /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
- 0.5, 0.02)
- .finished(),
- (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
@@ -574,11 +551,9 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/true,
@@ -589,16 +564,14 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/true,
/*estimate_tolerance=*/0.15,
- /*goal_tolerance=*/0.5,
+ /*goal_tolerance=*/0.8,
}),
// Try another spline, just in case the one I was using is special for
// some reason; this path will also go straight up to a target, to
@@ -606,11 +579,9 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
/*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
- (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 2b70ab7..449ce64 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -11,9 +11,9 @@
back_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, M_PI}, kFakeFov, fake_noise_,
constants::Field().targets(), {}) {}
-bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) {
- const double speed = (state(3, 0) + state(4, 0)) / 2.0;
- if (::std::abs(speed) < kMinDecisionSpeed) {
+bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) {
+ if (::std::abs(command_speed) < kMinDecisionSpeed) {
return false;
}
*robot_pose_.mutable_pos() << state.x(), state.y(), 0.0;
@@ -21,7 +21,7 @@
::aos::SizedArray<FakeCamera::TargetView,
y2019::constants::Field::kNumTargets>
target_views;
- if (speed > 0) {
+ if (command_speed > 0) {
target_views = front_viewer_.target_views();
} else {
target_views = back_viewer_.target_views();
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index d3f1a9d..965d7cc 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -29,14 +29,15 @@
TargetSelector();
- bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) override;
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) override;
Pose TargetPose() const override { return target_pose_; }
private:
- static constexpr double kFakeFov = M_PI;
+ static constexpr double kFakeFov = M_PI * 0.7;
// Longitudinal speed at which the robot must be going in order for us to make
// a decision.
- static constexpr double kMinDecisionSpeed = 0.1; // m/s
+ static constexpr double kMinDecisionSpeed = 0.7; // m/s
Pose robot_pose_;
Pose target_pose_;
// For the noise of our fake cameras, we only really care about the max
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 7c0ad7f..4b440c2 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -18,10 +18,12 @@
// Tests the target selector with:
// -The [x, y, theta, left_vel, right_vel] state to test at
+// -The current driver commanded speed.
// -Whether we expect to see a target.
// -If (1) is true, the pose we expect to get back.
struct TestParams {
State state;
+ double command_speed;
bool expect_target;
Pose expected_pose;
};
@@ -31,7 +33,8 @@
TargetSelector selector;
bool expect_target = GetParam().expect_target;
const State state = GetParam().state;
- ASSERT_EQ(expect_target, selector.UpdateSelection(state))
+ ASSERT_EQ(expect_target,
+ selector.UpdateSelection(state, GetParam().command_speed))
<< "We expected a return of " << expect_target << " at state "
<< state.transpose();
if (expect_target) {
@@ -54,31 +57,42 @@
::testing::Values(
// When we are far away from anything, we should not register any
// targets:
- TestParams{(State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(), false, {}},
+ TestParams{
+ (State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(), 1.0, false, {}},
// Aim for a human-player spot; at low speeds we should not register
// anything.
- TestParams{
- (State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(), false, {}},
- TestParams{
- (State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(), false, {}},
- TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(), true,
+ TestParams{(State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(),
+ 0.05,
+ false,
+ {}},
+ TestParams{(State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(),
+ -0.05,
+ false,
+ {}},
+ TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(), 1.0, true,
HPSlotLeft()},
// Put ourselves between the rocket and cargo ship; we should see the
// portal driving one direction and the near cargo ship port the other.
- TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(), true,
- CargoNearLeft()},
- TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(), true,
- CargoNearLeft()},
- TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(), true,
- RocketPortalLeft()},
- TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(), true,
- RocketPortalLeft()},
+ // We also command a speed opposite the current direction of motion and
+ // confirm that that behaves as expected.
+ TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(), 1.0,
+ true, CargoNearLeft()},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(), -1.0,
+ true, CargoNearLeft()},
+ TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(), -1.0,
+ true, RocketPortalLeft()},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(), 1.0,
+ true, RocketPortalLeft()},
// And we shouldn't see anything spinning in place:
- TestParams{
- (State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(), false, {}},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(),
+ 0.0,
+ false,
+ {}},
// Drive backwards off the field--we should not see anything.
- TestParams{
- (State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(), false, {}}));
+ TestParams{(State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(),
+ -1.0,
+ false,
+ {}}));
} // namespace testing
} // namespace control_loops