Added position control and profiling to drivetrain.
Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ee1efdd..73fb9a5 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -37,6 +37,13 @@
)
cc_library(
+ name = 'gear',
+ hdrs = [
+ 'gear.h',
+ ],
+)
+
+cc_library(
name = 'ssdrivetrain',
srcs = [
'ssdrivetrain.cc',
@@ -47,15 +54,18 @@
deps = [
':drivetrain_queue',
':drivetrain_config',
+ ':gear',
+ '//aos/common/controls:control_loop',
'//aos/common/controls:polytope',
- '//aos/common:math',
- '//aos/common/messages:robot_state',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:coerce_goal',
- '//frc971:shifter_hall_effect',
- '//aos/common/util:log_interval',
- '//aos/common/logging:queue_logging',
'//aos/common/logging:matrix_logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
+ '//aos/common/util:log_interval',
+ '//aos/common/util:trapezoid_profile',
+ '//aos/common:math',
+ '//frc971/control_loops:coerce_goal',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971:shifter_hall_effect',
],
)
@@ -70,6 +80,7 @@
deps = [
':drivetrain_queue',
':drivetrain_config',
+ ':gear',
'//aos/common/controls:polytope',
'//aos/common:math',
'//aos/common/messages:robot_state',
@@ -91,6 +102,7 @@
],
deps = [
':drivetrain_queue',
+ ':gear',
':polydrivetrain',
':ssdrivetrain',
'//aos/common/controls:control_loop',
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 12d2cbc..b4c04a3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -31,30 +31,92 @@
dt_config_(dt_config),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
- dt_closedloop_(dt_config_) {
+ dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
+ right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
+ left_high_requested_(dt_config_.default_high_gear),
+ right_high_requested_(dt_config_.default_high_gear) {
::aos::controls::HPolytope<0>::Init();
}
+int DrivetrainLoop::ControllerIndexFromGears() {
+ if (MaybeHigh(left_gear_)) {
+ if (MaybeHigh(right_gear_)) {
+ return 3;
+ } else {
+ return 2;
+ }
+ } else {
+ if (MaybeHigh(right_gear_)) {
+ return 1;
+ } else {
+ return 0;
+ }
+ }
+}
+
+Gear ComputeGear(double shifter_position,
+ const constants::ShifterHallEffect &shifter_config,
+ bool high_requested) {
+ if (shifter_position < shifter_config.clear_low) {
+ return Gear::LOW;
+ } else if (shifter_position > shifter_config.clear_high) {
+ return Gear::HIGH;
+ } else {
+ if (high_requested) {
+ return Gear::SHIFTING_UP;
+ } else {
+ return Gear::SHIFTING_DOWN;
+ }
+ }
+}
+
void DrivetrainLoop::RunIteration(
const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
const ::frc971::control_loops::DrivetrainQueue::Position *position,
::frc971::control_loops::DrivetrainQueue::Output *output,
::frc971::control_loops::DrivetrainQueue::Status *status) {
- bool bad_pos = false;
- if (position == nullptr) {
- LOG_INTERVAL(no_position_);
- bad_pos = true;
+ // TODO(austin): Put gear detection logic here.
+ switch (dt_config_.shifter_type) {
+ case ShifterType::SIMPLE_SHIFTER:
+ // Force the right controller for simple shifters since we assume that
+ // gear switching is instantaneous.
+ if (left_high_requested_) {
+ left_gear_ = Gear::HIGH;
+ } else {
+ left_gear_ = Gear::LOW;
+ }
+ if (right_high_requested_) {
+ right_gear_ = Gear::HIGH;
+ } else {
+ right_gear_ = Gear::LOW;
+ }
+ break;
+ case ShifterType::HALL_EFFECT_SHIFTER:
+ left_gear_ = ComputeGear(position->left_shifter_position,
+ dt_config_.left_drive, left_high_requested_);
+ right_gear_ = ComputeGear(position->right_shifter_position,
+ dt_config_.right_drive, right_high_requested_);
+ break;
}
- no_position_.Print();
- kf_.set_controller_index(dt_openloop_.controller_index());
+ kf_.set_controller_index(ControllerIndexFromGears());
+ {
+ GearLogging gear_logging;
+ gear_logging.left_state = static_cast<uint32_t>(left_gear_);
+ gear_logging.right_state = static_cast<uint32_t>(right_gear_);
+ gear_logging.left_loop_high = MaybeHigh(left_gear_);
+ gear_logging.right_loop_high = MaybeHigh(right_gear_);
+ gear_logging.controller_index = kf_.controller_index();
+ LOG_STRUCT(DEBUG, "state", gear_logging);
+ }
- bool gyro_valid = false;
+ // TODO(austin): Signal the current gear to both loops.
+
if (gyro_reading.FetchLatest()) {
LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
last_gyro_heading_ = gyro_reading->angle;
last_gyro_rate_ = gyro_reading->velocity;
- gyro_valid = true;
}
{
@@ -64,62 +126,69 @@
integrated_kf_heading_ += dt_config_.dt *
(kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) /
(dt_config_.robot_radius * 2.0);
+
+ // gyro_heading = (real_right - real_left) / width
+ // wheel_heading = (wheel_right - wheel_left) / width
+ // gyro_heading + offset = wheel_heading
+ // gyro_goal + offset = wheel_goal
+ // offset = wheel_heading - gyro_heading
+
+ // gyro_goal + wheel_heading - gyro_heading = wheel_goal
}
+ dt_openloop_.SetPosition(position, left_gear_, right_gear_);
+
bool control_loop_driving = false;
if (goal) {
- double wheel = goal->steering;
- double throttle = goal->throttle;
- bool quickturn = goal->quickturn;
- bool highgear = goal->highgear;
-
control_loop_driving = goal->control_loop_driving;
- double left_goal = goal->left_goal;
- double right_goal = goal->right_goal;
- dt_closedloop_.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
- goal->right_velocity_goal);
- dt_openloop_.SetGoal(wheel, throttle, quickturn, highgear);
+ dt_closedloop_.SetGoal(*goal);
+ dt_openloop_.SetGoal(*goal);
}
- if (!bad_pos) {
- const double left_encoder = position->left_encoder;
- const double right_encoder = position->right_encoder;
- if (gyro_valid) {
- dt_closedloop_.SetPosition(left_encoder, right_encoder,
- gyro_reading->angle);
- } else {
- dt_closedloop_.SetRawPosition(left_encoder, right_encoder);
- }
- }
- dt_openloop_.SetPosition(position);
dt_openloop_.Update();
if (control_loop_driving) {
- dt_closedloop_.Update(output == NULL, true);
- dt_closedloop_.SendMotors(output);
+ dt_closedloop_.Update(output != NULL);
+ dt_closedloop_.SetOutput(output);
} else {
- dt_openloop_.SendMotors(output);
- if (output) {
- dt_closedloop_.SetExternalMotors(output->left_voltage,
- output->right_voltage);
- }
- dt_closedloop_.Update(output == NULL, false);
+ dt_openloop_.SetOutput(output);
+ // TODO(austin): Set profile to current spot.
+ dt_closedloop_.Update(false);
}
+ // The output should now contain the shift request.
+
// set the output status of the control loop state
if (status) {
- status->robot_speed = dt_closedloop_.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop_.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop_.GetEstimatedRightEncoder();
+ status->robot_speed = (kf_.X_hat(1, 0) + kf_.X_hat(3, 0)) / 2.0;
- status->filtered_left_velocity = dt_closedloop_.loop().X_hat(1, 0);
- status->filtered_right_velocity = dt_closedloop_.loop().X_hat(3, 0);
- status->output_was_capped = dt_closedloop_.OutputWasCapped();
- status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
- status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
+ Eigen::Matrix<double, 2, 1> linear =
+ dt_closedloop_.LeftRightToLinear(kf_.X_hat());
+ Eigen::Matrix<double, 2, 1> angular =
+ dt_closedloop_.LeftRightToAngular(kf_.X_hat());
+
+ angular(0, 0) = integrated_kf_heading_;
+
+ Eigen::Matrix<double, 4, 1> gyro_left_right =
+ dt_closedloop_.AngularLinearToLeftRight(linear, angular);
+
+ status->estimated_left_position = gyro_left_right(0, 0);
+ status->estimated_right_position = gyro_left_right(2, 0);
+
+ status->estimated_left_velocity = gyro_left_right(1, 0);
+ status->estimated_right_velocity = gyro_left_right(3, 0);
+ status->output_was_capped = dt_closedloop_.output_was_capped();
+ status->uncapped_left_voltage = kf_.U_uncapped(0, 0);
+ status->uncapped_right_voltage = kf_.U_uncapped(1, 0);
+
+ status->left_voltage_error = kf_.X_hat(4, 0);
+ status->right_voltage_error = kf_.X_hat(5, 0);
+ status->estimated_angular_velocity_error = kf_.X_hat(6, 0);
+ status->estimated_heading = integrated_kf_heading_;
dt_openloop_.PopulateStatus(status);
+ dt_closedloop_.PopulateStatus(status);
}
double left_voltage = 0.0;
@@ -127,6 +196,8 @@
if (output) {
left_voltage = output->left_voltage;
right_voltage = output->right_voltage;
+ left_high_requested_ = output->left_high;
+ right_high_requested_ = output->right_high;
}
const double scalar = ::aos::robot_state->voltage_battery / 12.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 30548be..a9541b7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -3,14 +3,15 @@
#include "Eigen/Dense"
-#include "aos/common/controls/polytope.h"
#include "aos/common/controls/control_loop.h"
#include "aos/common/controls/polytope.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/util/log_interval.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "aos/common/util/log_interval.h"
namespace frc971 {
namespace control_loops {
@@ -26,6 +27,8 @@
::frc971::control_loops::DrivetrainQueue *my_drivetrain =
&::frc971::control_loops::drivetrain_queue);
+ int ControllerIndexFromGears();
+
protected:
// Executes one cycle of the control loop.
void RunIteration(
@@ -36,9 +39,6 @@
void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
- typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
- SimpleLogInterval no_position_ = SimpleLogInterval(
- ::aos::time::Time::InSeconds(0.25), WARNING, "no position");
double last_gyro_heading_ = 0.0;
double last_gyro_rate_ = 0.0;
@@ -48,10 +48,17 @@
PolyDrivetrain dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ // Current gears for each drive side.
+ Gear left_gear_;
+ Gear right_gear_;
+
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
double integrated_kf_heading_ = 0;
+
+ bool left_high_requested_;
+ bool right_high_requested_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 24b1d38..3ef64bf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -2,6 +2,14 @@
import "aos/common/controls/control_loops.q";
+// Parameters for the motion profiles.
+struct ProfileParameters {
+ // Maximum velocity for the profile.
+ float max_velocity;
+ // Maximum acceleration for the profile.
+ float max_acceleration;
+};
+
// For logging information about what the code is doing with the shifters.
struct GearLogging {
// Which controller is being used.
@@ -62,6 +70,11 @@
// controller is active.
double left_velocity_goal;
double right_velocity_goal;
+
+ // Motion profile parameters.
+ // The control loop will profile if these are all non-zero.
+ ProfileParameters linear;
+ ProfileParameters angular;
};
message Position {
@@ -101,12 +114,12 @@
double robot_speed;
// Estimated relative position of each drivetrain side (in meters).
- double filtered_left_position;
- double filtered_right_position;
+ double estimated_left_position;
+ double estimated_right_position;
// Estimated velocity of each drivetrain side (in m/s).
- double filtered_left_velocity;
- double filtered_right_velocity;
+ double estimated_left_velocity;
+ double estimated_right_velocity;
// The voltage we wanted to send to each drivetrain side last cycle.
double uncapped_left_voltage;
@@ -116,6 +129,23 @@
double left_velocity_goal;
double right_velocity_goal;
+ // The voltage error for the left and right sides.
+ double left_voltage_error;
+ double right_voltage_error;
+
+ // The profiled goal states.
+ double profiled_left_position_goal;
+ double profiled_right_position_goal;
+ double profiled_left_velocity_goal;
+ double profiled_right_velocity_goal;
+
+ // The KF offset
+ double estimated_angular_velocity_error;
+ // The KF estimated heading.
+ double estimated_heading;
+ // The KF wheel estimated heading.
+ //double estimated_wheel_heading;
+
// True if the output voltage was capped last cycle.
bool output_was_capped;
};
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 4ceda53..f2c4638 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -2,28 +2,30 @@
#include <memory>
-#include "gtest/gtest.h"
-#include "aos/common/network/team_number.h"
-#include "aos/common/controls/polytope.h"
#include "aos/common/controls/control_loop_test.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/time.h"
+#include "gtest/gtest.h"
-#include "y2016/constants.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/coerce_goal.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2016/constants.h"
#include "frc971/queues/gyro.q.h"
+#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
namespace testing {
+using ::aos::time::Time;
+
using ::y2016::control_loops::drivetrain::MakeDrivetrainPlant;
// TODO(Comran): Make one that doesn't depend on the actual values for a
@@ -47,13 +49,38 @@
::y2016::control_loops::drivetrain::kHighGearRatio,
::y2016::control_loops::drivetrain::kLowGearRatio,
- kThreeStateDriveShifter,
- kThreeStateDriveShifter,
- false};
+ kThreeStateDriveShifter, kThreeStateDriveShifter, false};
return kDrivetrainConfig;
};
+class DrivetrainPlant : public StateFeedbackPlant<4, 2, 2> {
+ public:
+ explicit DrivetrainPlant(StateFeedbackPlant<4, 2, 2> &&other)
+ : StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
+
+ void CheckU() override {
+ assert(U(0, 0) <= U_max(0, 0) + 0.00001 + left_voltage_offset_);
+ assert(U(0, 0) >= U_min(0, 0) - 0.00001 + left_voltage_offset_);
+ assert(U(1, 0) <= U_max(1, 0) + 0.00001 + right_voltage_offset_);
+ assert(U(1, 0) >= U_min(1, 0) - 0.00001 + right_voltage_offset_);
+ }
+
+ double left_voltage_offset() const { return left_voltage_offset_; }
+ void set_left_voltage_offset(double left_voltage_offset) {
+ left_voltage_offset_ = left_voltage_offset;
+ }
+
+ double right_voltage_offset() const { return right_voltage_offset_; }
+ void set_right_voltage_offset(double right_voltage_offset) {
+ right_voltage_offset_ = right_voltage_offset;
+ }
+
+ private:
+ double left_voltage_offset_ = 0.0;
+ double right_voltage_offset_ = 0.0;
+};
+
// Class which simulates the drivetrain and sends out queue messages containing
// the position.
class DrivetrainSimulation {
@@ -62,13 +89,15 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+ new DrivetrainPlant(MakeDrivetrainPlant())),
my_drivetrain_queue_(".frc971.control_loops.drivetrain", 0x8a8dde77,
".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status") {
+ ".frc971.control_loops.drivetrain.status"),
+ gyro_reading_(::frc971::sensors::gyro_reading.name()) {
Reinitialize();
+ last_U_.setZero();
}
// Resets the plant.
@@ -90,11 +119,27 @@
const double left_encoder = GetLeftPosition();
const double right_encoder = GetRightPosition();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
- position = my_drivetrain_queue_.position.MakeMessage();
- position->left_encoder = left_encoder;
- position->right_encoder = right_encoder;
- position.Send();
+ {
+ ::aos::ScopedMessagePtr<
+ ::frc971::control_loops::DrivetrainQueue::Position> position =
+ my_drivetrain_queue_.position.MakeMessage();
+ position->left_encoder = left_encoder;
+ position->right_encoder = right_encoder;
+ position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
+ position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
+ position.Send();
+ }
+
+ {
+ ::aos::ScopedMessagePtr<::frc971::sensors::GyroReading> gyro =
+ gyro_reading_.MakeMessage();
+ gyro->angle = (right_encoder - left_encoder) /
+ (::y2016::control_loops::drivetrain::kRobotRadius * 2.0);
+ gyro->velocity =
+ (drivetrain_plant_->X(3, 0) - drivetrain_plant_->X(1, 0)) /
+ (::y2016::control_loops::drivetrain::kRobotRadius * 2.0);
+ gyro.Send();
+ }
}
// Simulates the drivetrain moving for one timestep.
@@ -102,17 +147,57 @@
last_left_position_ = drivetrain_plant_->Y(0, 0);
last_right_position_ = drivetrain_plant_->Y(1, 0);
EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- drivetrain_plant_->mutable_U() << my_drivetrain_queue_.output->left_voltage,
+ drivetrain_plant_->mutable_U() = last_U_;
+ last_U_ << my_drivetrain_queue_.output->left_voltage,
my_drivetrain_queue_.output->right_voltage;
+ {
+ const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+ last_U_ *= scalar;
+ }
+ left_gear_high_ = my_drivetrain_queue_.output->left_high;
+ right_gear_high_ = my_drivetrain_queue_.output->right_high;
+
+ if (left_gear_high_) {
+ if (right_gear_high_) {
+ drivetrain_plant_->set_plant_index(3);
+ } else {
+ drivetrain_plant_->set_plant_index(2);
+ }
+ } else {
+ if (right_gear_high_) {
+ drivetrain_plant_->set_plant_index(1);
+ } else {
+ drivetrain_plant_->set_plant_index(0);
+ }
+ }
+
+ drivetrain_plant_->mutable_U(0, 0) +=
+ drivetrain_plant_->left_voltage_offset();
+ drivetrain_plant_->mutable_U(1, 0) +=
+ drivetrain_plant_->right_voltage_offset();
drivetrain_plant_->Update();
}
- ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+ void set_left_voltage_offset(double left_voltage_offset) {
+ drivetrain_plant_->set_left_voltage_offset(left_voltage_offset);
+ }
+ void set_right_voltage_offset(double right_voltage_offset) {
+ drivetrain_plant_->set_right_voltage_offset(right_voltage_offset);
+ }
+
+ ::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
private:
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
+ ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
+
double last_left_position_;
double last_right_position_;
+
+ Eigen::Matrix<double, 2, 1> last_U_;
+
+ bool left_gear_high_ = false;
+ bool right_gear_high_ = false;
};
class DrivetrainTest : public ::aos::testing::ControlLoopTest {
@@ -135,15 +220,30 @@
drivetrain_motor_(GetDrivetrainConfig(), &my_drivetrain_queue_),
drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
+ set_battery_voltage(12.0);
+ }
+
+ void RunIteration() {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ void RunForTime(const Time run_for) {
+ const auto end_time = Time::Now() + run_for;
+ while (Time::Now() < end_time) {
+ RunIteration();
+ }
}
void VerifyNearGoal() {
my_drivetrain_queue_.goal.FetchLatest();
my_drivetrain_queue_.position.FetchLatest();
EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
- drivetrain_motor_plant_.GetLeftPosition(), 1e-2);
+ drivetrain_motor_plant_.GetLeftPosition(), 1e-3);
EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
- drivetrain_motor_plant_.GetRightPosition(), 1e-2);
+ drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
@@ -156,12 +256,21 @@
.left_goal(-1.0)
.right_goal(1.0)
.Send();
- for (int i = 0; i < 200; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true);
- }
+ RunForTime(Time::InSeconds(1.0));
+ VerifyNearGoal();
+}
+
+// Tests that the drivetrain converges on a goal when under the effect of a
+// voltage offset/disturbance.
+TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0)
+ .Send();
+ drivetrain_motor_plant_.set_left_voltage_offset(1.0);
+ drivetrain_motor_plant_.set_right_voltage_offset(1.0);
+ RunForTime(Time::InSeconds(1.5));
VerifyNearGoal();
}
@@ -197,12 +306,7 @@
// Tests that never having a goal, but having driver's station messages, doesn't
// break.
TEST_F(DrivetrainTest, NoGoalWithRobotState) {
- for (int i = 0; i < 20; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true);
- }
+ RunForTime(Time::InSeconds(0.1));
}
// Tests that the robot successfully drives straight forward.
@@ -214,15 +318,12 @@
.right_goal(4.0)
.Send();
for (int i = 0; i < 500; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunIteration();
ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_FLOAT_EQ(my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+ EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage, 1e-4);
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
}
VerifyNearGoal();
}
@@ -236,13 +337,187 @@
.right_goal(3.9)
.Send();
for (int i = 0; i < 500; ++i) {
+ RunIteration();
+ ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that converting from a left, right position to a distance, angle
+// coordinate system and back returns the same answer.
+TEST_F(DrivetrainTest, LinearToAngularAndBack) {
+ StateFeedbackLoop<7, 2, 3> kf(
+ GetDrivetrainConfig().make_kf_drivetrain_loop());
+ double kf_heading = 0;
+ DrivetrainMotorsSS drivetrain_ss(GetDrivetrainConfig(), &kf, &kf_heading);
+
+ const double width = GetDrivetrainConfig().robot_radius * 2.0;
+
+ Eigen::Matrix<double, 7, 1> state;
+ state << 2, 3, 4, 5, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> linear = drivetrain_ss.LeftRightToLinear(state);
+
+ EXPECT_NEAR(3.0, linear(0, 0), 1e-6);
+ EXPECT_NEAR(4.0, linear(1, 0), 1e-6);
+
+ Eigen::Matrix<double, 2, 1> angular = drivetrain_ss.LeftRightToAngular(state);
+
+ EXPECT_NEAR(2.0 / width, angular(0, 0), 1e-6);
+ EXPECT_NEAR(2.0 / width, angular(1, 0), 1e-6);
+
+ Eigen::Matrix<double, 4, 1> back_state =
+ drivetrain_ss.AngularLinearToLeftRight(linear, angular);
+
+ for (int i = 0; i < 4; ++i) {
+ EXPECT_NEAR(state(i, 0), back_state(i, 0), 1e-8);
+ }
+}
+
+// Tests that a linear motion profile succeeds.
+TEST_F(DrivetrainTest, ProfileStraightForward) {
+ {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ goal = my_drivetrain_queue_.goal.MakeMessage();
+ goal->control_loop_driving = true;
+ goal->left_goal = 4.0;
+ goal->right_goal = 4.0;
+ goal->left_velocity_goal = 0.0;
+ goal->right_velocity_goal = 0.0;
+ goal->linear.max_velocity = 1.0;
+ goal->linear.max_acceleration = 3.0;
+ goal->angular.max_velocity = 1.0;
+ goal->angular.max_acceleration = 3.0;
+ goal.Send();
+ }
+
+ while (Time::Now() < Time::InSeconds(6)) {
+ RunIteration();
+ ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage, 1e-4);
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
+ EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
+ EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that an angular motion profile succeeds.
+TEST_F(DrivetrainTest, ProfileTurn) {
+ {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ goal = my_drivetrain_queue_.goal.MakeMessage();
+ goal->control_loop_driving = true;
+ goal->left_goal = -1.0;
+ goal->right_goal = 1.0;
+ goal->left_velocity_goal = 0.0;
+ goal->right_velocity_goal = 0.0;
+ goal->linear.max_velocity = 1.0;
+ goal->linear.max_acceleration = 3.0;
+ goal->angular.max_velocity = 1.0;
+ goal->angular.max_acceleration = 3.0;
+ goal.Send();
+ }
+
+ while (Time::Now() < Time::InSeconds(6)) {
+ RunIteration();
+ ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
+ -my_drivetrain_queue_.output->right_voltage, 1e-4);
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
+ EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
+ EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that a mixed turn drive saturated profile succeeds.
+TEST_F(DrivetrainTest, SaturatedTurnDrive) {
+ {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ goal = my_drivetrain_queue_.goal.MakeMessage();
+ goal->control_loop_driving = true;
+ goal->left_goal = 5.0;
+ goal->right_goal = 4.0;
+ goal->left_velocity_goal = 0.0;
+ goal->right_velocity_goal = 0.0;
+ goal->linear.max_velocity = 6.0;
+ goal->linear.max_acceleration = 4.0;
+ goal->angular.max_velocity = 2.0;
+ goal->angular.max_acceleration = 4.0;
+ goal.Send();
+ }
+
+ while (Time::Now() < Time::InSeconds(3)) {
+ RunIteration();
+ ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ }
+ VerifyNearGoal();
+}
+
+// Tests that being in teleop drive for a bit and then transitioning to closed
+// drive profiles nicely.
+TEST_F(DrivetrainTest, OpenLoopThenClosed) {
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .steering(0.0)
+ .throttle(1.0)
+ .highgear(true)
+ .quickturn(false)
+ .Send();
+
+ RunForTime(Time::InSeconds(1.0));
+
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .steering(0.0)
+ .throttle(-0.3)
+ .highgear(true)
+ .quickturn(false)
+ .Send();
+
+ RunForTime(Time::InSeconds(1.0));
+
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .highgear(true)
+ .quickturn(false)
+ .Send();
+
+ RunForTime(Time::InSeconds(10.0));
+
+ {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ goal = my_drivetrain_queue_.goal.MakeMessage();
+ goal->control_loop_driving = true;
+ goal->left_goal = 5.0;
+ goal->right_goal = 4.0;
+ goal->left_velocity_goal = 0.0;
+ goal->right_velocity_goal = 0.0;
+ goal->linear.max_velocity = 1.0;
+ goal->linear.max_acceleration = 2.0;
+ goal->angular.max_velocity = 1.0;
+ goal->angular.max_acceleration = 2.0;
+ goal.Send();
+ }
+
+ const auto end_time = Time::Now() + Time::InSeconds(4);
+ while (Time::Now() < end_time) {
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
SimulateTimestep(true);
ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
+ EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
+ EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
}
VerifyNearGoal();
}
@@ -349,6 +624,8 @@
EXPECT_EQ(1.0, output(1, 0));
}
+// TODO(austin): Make sure the profile reset code when we disable works.
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/gear.h b/frc971/control_loops/drivetrain/gear.h
new file mode 100644
index 0000000..8fabb69
--- /dev/null
+++ b/frc971/control_loops/drivetrain/gear.h
@@ -0,0 +1,25 @@
+#ifndef FRC971_CONTROL_LOOPS_GEAR_H_
+#define FRC971_CONTROL_LOOPS_GEAR_H_
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// The state of the shift.
+enum class Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
+
+// True if the the robot might or is trying to be in high gear.
+inline bool MaybeHigh(Gear g) {
+ return g == Gear::HIGH || g == Gear::SHIFTING_UP;
+}
+
+// True if the shifter is engaged and ready for action.
+inline bool IsInGear(Gear gear) {
+ return gear == Gear::LOW || gear == Gear::HIGH;
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_GEAR_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index f088bff..8925a40 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -36,10 +36,8 @@
wheel_(0.0),
throttle_(0.0),
quickturn_(false),
- stale_count_(0),
- position_time_delta_(dt_config.dt),
- left_gear_(LOW),
- right_gear_(LOW),
+ left_gear_(Gear::LOW),
+ right_gear_(Gear::LOW),
counter_(0),
dt_config_(dt_config) {
last_position_.Zero();
@@ -64,70 +62,60 @@
// Not in gear, so speed-match to destination gear.
switch (gear) {
- case HIGH:
- case SHIFTING_UP:
+ case Gear::HIGH:
+ case Gear::SHIFTING_UP:
return high_gear_speed;
- case LOW:
- case SHIFTING_DOWN:
+ case Gear::LOW:
+ case Gear::SHIFTING_DOWN:
default:
return low_gear_speed;
break;
}
}
-PolyDrivetrain::Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear,
- Gear current_gear) {
+Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear, Gear current_gear) {
const Gear shift_up =
(dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
- ? SHIFTING_UP
- : HIGH;
+ ? Gear::SHIFTING_UP
+ : Gear::HIGH;
const Gear shift_down =
(dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
- ? SHIFTING_DOWN
- : LOW;
+ ? Gear::SHIFTING_DOWN
+ : Gear::LOW;
if (current_gear != requested_gear) {
if (IsInGear(current_gear)) {
- if (requested_gear == HIGH) {
- if (current_gear != HIGH) {
+ if (requested_gear == Gear::HIGH) {
+ if (current_gear != Gear::HIGH) {
current_gear = shift_up;
}
} else {
- if (current_gear != LOW) {
+ if (current_gear != Gear::LOW) {
current_gear = shift_down;
}
}
} else {
- if (requested_gear == HIGH && current_gear == SHIFTING_DOWN) {
- current_gear = SHIFTING_UP;
- } else if (requested_gear == LOW && current_gear == SHIFTING_UP) {
- current_gear = SHIFTING_DOWN;
+ if (requested_gear == Gear::HIGH && current_gear == Gear::SHIFTING_DOWN) {
+ current_gear = Gear::SHIFTING_UP;
+ } else if (requested_gear == Gear::LOW &&
+ current_gear == Gear::SHIFTING_UP) {
+ current_gear = Gear::SHIFTING_DOWN;
}
}
}
return current_gear;
}
-void PolyDrivetrain::UpdateGears(Gear requested_gear) {
- left_gear_ = UpdateSingleGear(requested_gear, left_gear_);
- right_gear_ = UpdateSingleGear(requested_gear, right_gear_);
-}
+void PolyDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ const double wheel = goal.steering;
+ const double throttle = goal.throttle;
+ const bool quickturn = goal.quickturn;
+ const bool highgear = goal.highgear;
-void PolyDrivetrain::SetGoal(double wheel, double throttle, bool quickturn,
- bool highgear) {
const double kWheelNonLinearity = 0.4;
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
- if (dt_config_.shifter_type == ShifterType::SIMPLE_SHIFTER) {
- // Force the right controller for simple shifters since we assume that
- // gear switching is instantaneous.
- if (highgear) {
- loop_->set_controller_index(3);
- } else {
- loop_->set_controller_index(0);
- }
- }
-
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
wheel_ = 2.0 * wheel - wheel_;
@@ -142,76 +130,19 @@
throttle);
}
- UpdateGears(highgear ? HIGH : LOW);
+ Gear requested_gear = highgear ? Gear::HIGH : Gear::LOW;
+
+ left_gear_ = PolyDrivetrain::UpdateSingleGear(requested_gear, left_gear_);
+ right_gear_ = PolyDrivetrain::UpdateSingleGear(requested_gear, right_gear_);
}
void PolyDrivetrain::SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position) {
- if (position == NULL) {
- ++stale_count_;
- } else {
- last_position_ = position_;
- position_ = *position;
- position_time_delta_ = (stale_count_ + 1) * dt_config_.dt;
- stale_count_ = 0;
- }
-
- if (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER && position) {
- GearLogging gear_logging;
- // Switch to the correct controller.
- const double left_middle_shifter_position =
- (dt_config_.left_drive.clear_high + dt_config_.left_drive.clear_low) /
- 2.0;
- const double right_middle_shifter_position =
- (dt_config_.right_drive.clear_high + dt_config_.right_drive.clear_low) /
- 2.0;
-
- if (position->left_shifter_position < left_middle_shifter_position ||
- left_gear_ == LOW) {
- if (position->right_shifter_position < right_middle_shifter_position ||
- right_gear_ == LOW) {
- gear_logging.left_loop_high = false;
- gear_logging.right_loop_high = false;
- loop_->set_controller_index(gear_logging.controller_index = 0);
- } else {
- gear_logging.left_loop_high = false;
- gear_logging.right_loop_high = true;
- loop_->set_controller_index(gear_logging.controller_index = 1);
- }
- } else {
- if (position->right_shifter_position < right_middle_shifter_position ||
- right_gear_ == LOW) {
- gear_logging.left_loop_high = true;
- gear_logging.right_loop_high = false;
- loop_->set_controller_index(gear_logging.controller_index = 2);
- } else {
- gear_logging.left_loop_high = true;
- gear_logging.right_loop_high = true;
- loop_->set_controller_index(gear_logging.controller_index = 3);
- }
- }
-
- if (position->left_shifter_position > dt_config_.left_drive.clear_high &&
- left_gear_ == SHIFTING_UP) {
- left_gear_ = HIGH;
- }
- if (position->left_shifter_position < dt_config_.left_drive.clear_low &&
- left_gear_ == SHIFTING_DOWN) {
- left_gear_ = LOW;
- }
- if (position->right_shifter_position > dt_config_.right_drive.clear_high &&
- right_gear_ == SHIFTING_UP) {
- right_gear_ = HIGH;
- }
- if (position->right_shifter_position < dt_config_.right_drive.clear_low &&
- right_gear_ == SHIFTING_DOWN) {
- right_gear_ = LOW;
- }
-
- gear_logging.left_state = left_gear_;
- gear_logging.right_state = right_gear_;
- LOG_STRUCT(DEBUG, "state", gear_logging);
- }
+ const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ Gear left_gear, Gear right_gear) {
+ left_gear_ = left_gear;
+ right_gear_ = right_gear;
+ last_position_ = position_;
+ position_ = *position;
}
double PolyDrivetrain::FilterVelocity(double throttle) const {
@@ -331,11 +262,10 @@
}
} else {
const double current_left_velocity =
- (position_.left_encoder - last_position_.left_encoder) /
- position_time_delta_;
+ (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
const double current_right_velocity =
(position_.right_encoder - last_position_.right_encoder) /
- position_time_delta_;
+ dt_config_.dt;
const double left_motor_speed =
MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
current_left_velocity, left_gear_);
@@ -380,13 +310,13 @@
}
}
-void PolyDrivetrain::SendMotors(
+void PolyDrivetrain::SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output) {
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
- output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
- output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
+ output->left_high = MaybeHigh(left_gear_);
+ output->right_high = MaybeHigh(right_gear_);
}
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 689e76f..1ea38a7 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -3,6 +3,7 @@
#include "aos/common/controls/polytope.h"
+#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -13,32 +14,21 @@
class PolyDrivetrain {
public:
- enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
-
PolyDrivetrain(const DrivetrainConfig &dt_config,
StateFeedbackLoop<7, 2, 3> *kf);
int controller_index() const { return loop_->controller_index(); }
- bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
-
// Computes the speed of the motor given the hall effect position and the
// speed of the robot.
double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
double shifter_position, double velocity, Gear gear);
- // Computes the states of the shifters for the left and right drivetrain sides
- // given a requested state.
- void UpdateGears(Gear requested_gear);
-
- // Computes the next state of a shifter given the current state and the
- // requested state.
- Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
-
- void SetGoal(double wheel, double throttle, bool quickturn, bool highgear);
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
void SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position);
+ const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ Gear left_gear, Gear right_gear);
double FilterVelocity(double throttle) const;
@@ -46,10 +36,14 @@
void Update();
- void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
+ void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+ // Computes the next state of a shifter given the current state and the
+ // requested state.
+ Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
+
private:
StateFeedbackLoop<7, 2, 3> *kf_;
@@ -61,10 +55,10 @@
double wheel_;
double throttle_;
bool quickturn_;
- int stale_count_;
- double position_time_delta_;
+
Gear left_gear_;
Gear right_gear_;
+
::frc971::control_loops::DrivetrainQueue::Position last_position_;
::frc971::control_loops::DrivetrainQueue::Position position_;
int counter_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 19bbef1..c1db075 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -15,35 +15,31 @@
using ::frc971::control_loops::DoCoerceGoal;
-DrivetrainMotorsSS::LimitedDrivetrainLoop::LimitedDrivetrainLoop(
- StateFeedbackLoop<4, 2, 2> &&loop)
- : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
- U_poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
- .finished(),
- (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
- .finished()) {
- ::aos::controls::HPolytope<0>::Init();
- T_ << 1, 1, 1, -1;
- T_inverse_ = T_.inverse();
+void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+ output_was_capped_ =
+ ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
+
+ if (output_was_capped_) {
+ *U *= 12.0 / kf_->U_uncapped().lpNorm<Eigen::Infinity>();
+ }
}
// This intentionally runs the U-capping code even when it's unnecessary to help
// make it more deterministic. Only running it when one or both sides want
// out-of-range voltages could lead to things like running out of CPU under
// certain situations, which would be bad.
-void DrivetrainMotorsSS::LimitedDrivetrainLoop::CapU() {
- output_was_capped_ = ::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0;
+void DrivetrainMotorsSS::PolyCapU(Eigen::Matrix<double, 2, 1> *U) {
+ output_was_capped_ =
+ ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
- const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
+ const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
- LOG_MATRIX(DEBUG, "U at start", U());
- LOG_MATRIX(DEBUG, "R at start", R());
- LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
+ LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
+ position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
+ velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -53,8 +49,13 @@
velocity_error << error(1, 0), error(3, 0);
LOG_MATRIX(DEBUG, "error", error);
- const ::aos::controls::HPolytope<2> pos_poly(U_poly_, position_K * T_,
- -velocity_K * velocity_error);
+
+ Eigen::Matrix<double, 2, 1> U_integral;
+ U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
+
+ const ::aos::controls::HPolytope<2> pos_poly(
+ U_poly_, position_K * T_,
+ -velocity_K * velocity_error + U_integral - kf_->ff_U());
Eigen::Matrix<double, 2, 1> adjusted_pos_error;
{
@@ -99,86 +100,164 @@
}
}
- mutable_U() =
- velocity_K * velocity_error + position_K * T_ * adjusted_pos_error;
- LOG_MATRIX(DEBUG, "U is now", U());
+ *U = -U_integral + velocity_K *velocity_error +
+ position_K *T_ *adjusted_pos_error + kf_->ff_U();
if (!output_was_capped_) {
- if ((U() - U_uncapped()).norm() > 0.0001) {
+ if ((*U - kf_->U_uncapped()).norm() > 0.0001) {
LOG(FATAL, "U unnecessarily capped\n");
}
}
}
-DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config)
- : loop_(
- new LimitedDrivetrainLoop(dt_config.make_drivetrain_loop())),
- filtered_offset_(0.0),
- gyro_(0.0),
- left_goal_(0.0),
- right_goal_(0.0),
- raw_left_(0.0),
- raw_right_(0.0),
- dt_config_(dt_config) {
- // High gear on both.
- loop_->set_controller_index(3);
+DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
+ StateFeedbackLoop<7, 2, 3> *kf,
+ double *integrated_kf_heading)
+ : dt_config_(dt_config),
+ kf_(kf),
+ U_poly_(
+ (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()),
+ linear_profile_(::aos::controls::kLoopFrequency),
+ angular_profile_(::aos::controls::kLoopFrequency),
+ integrated_kf_heading_(integrated_kf_heading) {
+ ::aos::controls::HPolytope<0>::Init();
+ T_ << 1, 1, 1, -1;
+ T_inverse_ = T_.inverse();
+ unprofiled_goal_.setZero();
}
-void DrivetrainMotorsSS::SetGoal(double left, double left_velocity,
- double right, double right_velocity) {
- left_goal_ = left;
- right_goal_ = right;
- loop_->mutable_R() << left, left_velocity, right, right_velocity;
-}
-void DrivetrainMotorsSS::SetRawPosition(double left, double right) {
- raw_right_ = right;
- raw_left_ = left;
- Eigen::Matrix<double, 2, 1> Y;
- Y << left + filtered_offset_, right - filtered_offset_;
- loop_->Correct(Y);
-}
-void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
- // Decay the offset quickly because this gyro is great.
- const double offset = (right - left) / 2.0 - gyro * dt_config_.robot_radius;
- filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
- gyro_ = gyro;
- SetRawPosition(left, right);
+void DrivetrainMotorsSS::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ unprofiled_goal_ << goal.left_goal, goal.left_velocity_goal, goal.right_goal,
+ goal.right_velocity_goal, 0.0, 0.0, 0.0;
+
+ use_profile_ =
+ !kf_->Kff().isZero(0) &&
+ (goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
+ goal.angular.max_velocity != 0.0 &&
+ goal.angular.max_acceleration != 0.0);
+ linear_profile_.set_maximum_velocity(goal.linear.max_velocity);
+ linear_profile_.set_maximum_acceleration(goal.linear.max_acceleration);
+ angular_profile_.set_maximum_velocity(goal.angular.max_velocity);
+ angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
}
-void DrivetrainMotorsSS::SetExternalMotors(double left_voltage,
- double right_voltage) {
- loop_->mutable_U() << left_voltage, right_voltage;
+// (left + right) / 2 = linear
+// (right - left) / width = angular
+
+Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToLinear(
+ const Eigen::Matrix<double, 7, 1> &left_right) {
+ Eigen::Matrix<double, 2, 1> linear;
+ linear << (left_right(0, 0) + left_right(2, 0)) / 2.0,
+ (left_right(1, 0) + left_right(3, 0)) / 2.0;
+ return linear;
}
-void DrivetrainMotorsSS::Update(bool stop_motors, bool enable_control_loop) {
+Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToAngular(
+ const Eigen::Matrix<double, 7, 1> &left_right) {
+ Eigen::Matrix<double, 2, 1> angular;
+ angular << (left_right(2, 0) - left_right(0, 0)) /
+ (dt_config_.robot_radius * 2.0),
+ (left_right(3, 0) - left_right(1, 0)) / (dt_config_.robot_radius * 2.0);
+ return angular;
+}
+
+Eigen::Matrix<double, 4, 1> DrivetrainMotorsSS::AngularLinearToLeftRight(
+ const Eigen::Matrix<double, 2, 1> &linear,
+ const Eigen::Matrix<double, 2, 1> &angular) {
+ Eigen::Matrix<double, 2, 1> scaled_angle = angular * dt_config_.robot_radius;
+ Eigen::Matrix<double, 4, 1> state;
+ state << linear(0, 0) - scaled_angle(0, 0), linear(1, 0) - scaled_angle(1, 0),
+ linear(0, 0) + scaled_angle(0, 0), linear(1, 0) + scaled_angle(1, 0);
+ return state;
+}
+
+void DrivetrainMotorsSS::Update(bool enable_control_loop) {
if (enable_control_loop) {
- loop_->Update(stop_motors);
- } else {
- if (stop_motors) {
- loop_->mutable_U().setZero();
- loop_->mutable_U_uncapped().setZero();
+ // Update profiles.
+ Eigen::Matrix<double, 2, 1> unprofiled_linear =
+ LeftRightToLinear(unprofiled_goal_);
+ Eigen::Matrix<double, 2, 1> unprofiled_angular =
+ LeftRightToAngular(unprofiled_goal_);
+
+ Eigen::Matrix<double, 2, 1> wheel_heading =
+ LeftRightToAngular(kf_->X_hat());
+
+ Eigen::Matrix<double, 2, 1> next_linear;
+ Eigen::Matrix<double, 2, 1> next_angular;
+
+ if (use_profile_) {
+ next_linear = linear_profile_.Update(unprofiled_linear(0, 0),
+ unprofiled_linear(1, 0));
+ next_angular = angular_profile_.Update(unprofiled_angular(0, 0),
+ unprofiled_angular(1, 0));
+
+ } else {
+ next_angular = unprofiled_angular;
+ next_linear = unprofiled_linear;
}
- loop_->UpdateObserver(loop_->U());
+
+ next_angular(0, 0) += wheel_heading(0, 0) - *integrated_kf_heading_;
+
+ kf_->mutable_next_R().block<4, 1>(0, 0) =
+ AngularLinearToLeftRight(next_linear, next_angular);
+
+ kf_->mutable_next_R().block<3, 1>(4, 0) =
+ unprofiled_goal_.block<3, 1>(4, 0);
+
+ if (!use_profile_) {
+ kf_->mutable_R() = kf_->next_R();
+ }
+
+ // Run the controller.
+ Eigen::Matrix<double, 2, 1> U = kf_->ControllerOutput();
+
+ kf_->mutable_U_uncapped() = kf_->mutable_U() = U;
+ ScaleCapU(&kf_->mutable_U());
+
+ // Now update the feed forwards.
+ kf_->UpdateFFReference();
+
+ // Now, move the profile if things didn't go perfectly.
+ if (use_profile_ &&
+ (kf_->U() - kf_->U_uncapped()).lpNorm<Eigen::Infinity>() > 1e-4) {
+ linear_profile_.MoveCurrentState(LeftRightToLinear(kf_->R()));
+ angular_profile_.MoveCurrentState(LeftRightToAngular(kf_->R()));
+ }
+ } else {
+ unprofiled_goal_(0, 0) = kf_->X_hat()(0, 0);
+ unprofiled_goal_(1, 0) = 0.0;
+ unprofiled_goal_(2, 0) = kf_->X_hat()(2, 0);
+ unprofiled_goal_(3, 0) = 0.0;
+
+ linear_profile_.MoveCurrentState(LeftRightToLinear(unprofiled_goal_));
+ angular_profile_.MoveCurrentState(LeftRightToAngular(unprofiled_goal_));
+
+ kf_->mutable_next_R() = unprofiled_goal_;
+ kf_->mutable_R() = unprofiled_goal_;
}
- ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
- LOG_MATRIX(DEBUG, "E", E);
}
-double DrivetrainMotorsSS::GetEstimatedRobotSpeed() const {
- // lets just call the average of left and right velocities close enough
- return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
-}
-
-void DrivetrainMotorsSS::SendMotors(
+void DrivetrainMotorsSS::SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output) const {
if (output) {
- output->left_voltage = loop_->U(0, 0);
- output->right_voltage = loop_->U(1, 0);
+ output->left_voltage = kf_->U(0, 0);
+ output->right_voltage = kf_->U(1, 0);
output->left_high = true;
output->right_high = true;
}
}
+void DrivetrainMotorsSS::PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+ status->profiled_left_position_goal = kf_->next_R(0, 0);
+ status->profiled_left_velocity_goal = kf_->next_R(1, 0);
+ status->profiled_right_position_goal = kf_->next_R(2, 0);
+ status->profiled_right_velocity_goal = kf_->next_R(3, 0);
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 193dbb2..419b401 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -1,9 +1,11 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
-#include "aos/common/controls/polytope.h"
#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/controls/polytope.h"
#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
@@ -16,74 +18,62 @@
class DrivetrainMotorsSS {
public:
- class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
- public:
- LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop);
+ DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
+ StateFeedbackLoop<7, 2, 3> *kf,
+ double *integrated_kf_heading);
- bool output_was_capped() const {
- return output_was_capped_;
- }
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
- private:
- void CapU() override;
+ // Computes the power to send out as part of the controller. Should be called
+ // when disabled (with enable_control_loop false) so the profiles get computed
+ // correctly.
+ // enable_control_loop includes the actual enable bit and if the loop will go
+ // out to hw.
+ void Update(bool enable_control_loop);
- // Reprsents +/- full power on each motor in U-space, aka the square from
- // (-12, -12) to (12, 12).
- const ::aos::controls::HPolytope<2> U_poly_;
+ bool output_was_capped() const { return output_was_capped_; }
- // multiplying by T converts [left_error, right_error] to
- // [left_right_error_difference, total_distance_error].
- Eigen::Matrix<double, 2, 2> T_, T_inverse_;
-
- bool output_was_capped_ = false;
- };
-
- DrivetrainMotorsSS(const DrivetrainConfig &dt_config);
-
- void SetGoal(double left, double left_velocity, double right,
- double right_velocity);
-
- void SetRawPosition(double left, double right);
-
- void SetPosition(double left, double right, double gyro);
-
- void SetExternalMotors(double left_voltage, double right_voltage);
-
- void Update(bool stop_motors, bool enable_control_loop);
-
- double GetEstimatedRobotSpeed() const;
-
- double GetEstimatedLeftEncoder() const {
- return loop_->X_hat(0, 0);
- }
-
- double left_velocity() const { return loop_->X_hat(1, 0); }
- double right_velocity() const { return loop_->X_hat(3, 0); }
-
- double GetEstimatedRightEncoder() const {
- return loop_->X_hat(2, 0);
- }
-
- bool OutputWasCapped() const {
- return loop_->output_was_capped();
- }
-
- void SendMotors(
+ void SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output) const;
+ void PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const;
- const LimitedDrivetrainLoop &loop() const { return *loop_; }
+ // Converts the robot state to a linear distance position, velocity.
+ Eigen::Matrix<double, 2, 1> LeftRightToLinear(
+ const Eigen::Matrix<double, 7, 1> &left_right);
+ // Converts the robot state to an anglular distance, velocity.
+ Eigen::Matrix<double, 2, 1> LeftRightToAngular(
+ const Eigen::Matrix<double, 7, 1> &left_right);
+
+ // Converts the linear and angular position, velocity to the top 4 states of
+ // the robot state.
+ Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
+ const Eigen::Matrix<double, 2, 1> &linear,
+ const Eigen::Matrix<double, 2, 1> &angular);
private:
- ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
-
- double filtered_offset_;
- double gyro_;
- double left_goal_;
- double right_goal_;
- double raw_left_;
- double raw_right_;
+ void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
+ void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
const DrivetrainConfig dt_config_;
+ StateFeedbackLoop<7, 2, 3> *kf_;
+ Eigen::Matrix<double, 7, 1> unprofiled_goal_;
+
+ // Reprsents +/- full power on each motor in U-space, aka the square from
+ // (-12, -12) to (12, 12).
+ const ::aos::controls::HPolytope<2> U_poly_;
+
+ // multiplying by T converts [left_error, right_error] to
+ // [left_right_error_difference, total_distance_error].
+ Eigen::Matrix<double, 2, 2> T_, T_inverse_;
+
+ aos::util::TrapezoidProfile linear_profile_, angular_profile_;
+
+ bool output_was_capped_ = false;
+
+ bool use_profile_ = false;
+
+ double *integrated_kf_heading_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index a51564b..e78a786 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -416,6 +416,12 @@
}
UpdateObserver(U_);
+
+ UpdateFFReference();
+ }
+
+ // Updates R() after any CapU operations happen on U().
+ void UpdateFFReference() {
ff_U_ -= U_uncapped() - U();
if (!Kff().isZero(0)) {
R_ = A() * R() + B() * ff_U_;
diff --git a/y2012/control_loops/drivetrain/drivetrain.cc b/y2012/control_loops/drivetrain/drivetrain.cc
index e97e440..0a2eef0 100644
--- a/y2012/control_loops/drivetrain/drivetrain.cc
+++ b/y2012/control_loops/drivetrain/drivetrain.cc
@@ -100,11 +100,11 @@
// set the output status of the control loop state
if (status) {
status->robot_speed = dt_closedloop_.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop_.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop_.GetEstimatedRightEncoder();
+ status->estimated_left_position = dt_closedloop_.GetEstimatedLeftEncoder();
+ status->estimated_right_position = dt_closedloop_.GetEstimatedRightEncoder();
- status->filtered_left_velocity = dt_closedloop_.loop().X_hat(1, 0);
- status->filtered_right_velocity = dt_closedloop_.loop().X_hat(3, 0);
+ status->estimated_left_velocity = dt_closedloop_.loop().X_hat(1, 0);
+ status->estimated_right_velocity = dt_closedloop_.loop().X_hat(3, 0);
status->output_was_capped = dt_closedloop_.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
diff --git a/y2012/control_loops/drivetrain/drivetrain.q b/y2012/control_loops/drivetrain/drivetrain.q
index 819a28f..44080da 100644
--- a/y2012/control_loops/drivetrain/drivetrain.q
+++ b/y2012/control_loops/drivetrain/drivetrain.q
@@ -101,13 +101,13 @@
// Estimated speed of the center of the robot in m/s (positive forwards).
double robot_speed;
// Estimated relative position of the left side in meters.
- double filtered_left_position;
+ double estimated_left_position;
// Estimated relative position of the right side in meters.
- double filtered_right_position;
+ double estimated_right_position;
// Estimated velocity of the left side in m/s.
- double filtered_left_velocity;
+ double estimated_left_velocity;
// Estimated velocity of the left side in m/s.
- double filtered_right_velocity;
+ double estimated_right_velocity;
// The voltage we wanted to send to the left side last cycle.
double uncapped_left_voltage;
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 6c7427b..037fab9 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -60,7 +60,7 @@
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
- (status.filtered_left_position + status.filtered_right_position -
+ (status.estimated_left_position + status.estimated_right_position -
params.left_initial_position - params.right_initial_position) /
2.0);
} else {
@@ -143,11 +143,11 @@
const double left_error =
::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->filtered_left_position -
+ ->estimated_left_position -
(left_goal_state(0, 0) + params.left_initial_position));
const double right_error =
::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->filtered_right_position -
+ ->estimated_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error = ::std::abs(
::frc971::control_loops::drivetrain_queue.status->robot_speed);
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index 9c39f27..5860407 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -163,10 +163,10 @@
::frc971::control_loops::drivetrain_queue.status.FetchAnother();
double left_error = ::std::abs(
left_initial_position -
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position);
+ ::frc971::control_loops::drivetrain_queue.status->estimated_left_position);
double right_error = ::std::abs(
right_initial_position -
- ::frc971::control_loops::drivetrain_queue.status->filtered_right_position);
+ ::frc971::control_loops::drivetrain_queue.status->estimated_right_position);
const double kPositionThreshold = 0.05 + distance;
if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
LOG(INFO, "At the goal\n");
@@ -214,9 +214,9 @@
void InitializeEncoders() {
::frc971::control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position;
+ ::frc971::control_loops::drivetrain_queue.status->estimated_left_position;
right_initial_position =
- ::frc971::control_loops::drivetrain_queue.status->filtered_right_position;
+ ::frc971::control_loops::drivetrain_queue.status->estimated_right_position;
}
void WaitUntilClawDone() {
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index 2c6d477..4894a51 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -30,7 +30,9 @@
constants::GetValues().low_gear_ratio,
constants::GetValues().left_drive,
constants::GetValues().right_drive,
- true};
+ true,
+ // TODO(austin): Switch over to using the profle.
+ false};
return kDrivetrainConfig;
};
diff --git a/y2014_bot3/actions/drivetrain_action.cc b/y2014_bot3/actions/drivetrain_action.cc
index 93b3d46..bef5bd6 100644
--- a/y2014_bot3/actions/drivetrain_action.cc
+++ b/y2014_bot3/actions/drivetrain_action.cc
@@ -55,7 +55,7 @@
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
- (status.filtered_left_position + status.filtered_right_position) /
+ (status.estimated_left_position + status.estimated_right_position) /
2.0);
} else {
static const double divisor = K(0, 0) + K(0, 2);
@@ -133,10 +133,10 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- control_loops::drivetrain.status->filtered_left_position -
+ control_loops::drivetrain.status->estimated_left_position -
(left_goal_state(0, 0) + action_q_->goal->left_initial_position));
const double right_error = ::std::abs(
- control_loops::drivetrain.status->filtered_right_position -
+ control_loops::drivetrain.status->estimated_right_position -
(right_goal_state(0, 0) + action_q_->goal->right_initial_position));
const double velocity_error =
::std::abs(control_loops::drivetrain.status->robot_speed);
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
index bf5a3cb..3724cbe 100644
--- a/y2014_bot3/autonomous/auto.cc
+++ b/y2014_bot3/autonomous/auto.cc
@@ -49,9 +49,9 @@
void InitializeEncoders() {
control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain_queue.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->estimated_left_position;
right_initial_position =
- control_loops::drivetrain_queue.status->filtered_right_position;
+ control_loops::drivetrain_queue.status->estimated_right_position;
}
void HandleAuto() {
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.cc b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
index e9f425d..429338f 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
@@ -556,11 +556,11 @@
// set the output status of the control loop state
if (status) {
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->estimated_left_position = dt_closedloop.GetEstimatedLeftEncoder();
+ status->estimated_right_position = dt_closedloop.GetEstimatedRightEncoder();
- status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
- status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
+ status->estimated_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+ status->estimated_right_velocity = dt_closedloop.loop().X_hat(3, 0);
status->output_was_capped = dt_closedloop.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index f5f53bb..1b97f81 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -79,13 +79,13 @@
// Estimated speed of the center of the robot in m/s (positive forwards).
double robot_speed;
// Estimated relative position of the left side in meters.
- double filtered_left_position;
+ double estimated_left_position;
// Estimated relative position of the right side in meters.
- double filtered_right_position;
+ double estimated_right_position;
// Estimated velocity of the left side in m/s.
- double filtered_left_velocity;
+ double estimated_left_velocity;
// Estimated velocity of the right side in m/s.
- double filtered_right_velocity;
+ double estimated_right_velocity;
// The voltage we wanted to send to the left side last cycle.
double uncapped_left_voltage;
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 9ef81c5..5ace60d 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -56,7 +56,7 @@
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
- (status.filtered_left_position + status.filtered_right_position -
+ (status.estimated_left_position + status.estimated_right_position -
params.left_initial_position - params.right_initial_position) /
2.0);
} else {
@@ -138,10 +138,10 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- control_loops::drivetrain_queue.status->filtered_left_position -
+ control_loops::drivetrain_queue.status->estimated_left_position -
(left_goal_state(0, 0) + params.left_initial_position));
const double right_error = ::std::abs(
- control_loops::drivetrain_queue.status->filtered_right_position -
+ control_loops::drivetrain_queue.status->estimated_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error =
::std::abs(control_loops::drivetrain_queue.status->robot_speed);
diff --git a/y2015/autonomous/auto.cc b/y2015/autonomous/auto.cc
index 4e8aa59..4ab5a41 100644
--- a/y2015/autonomous/auto.cc
+++ b/y2015/autonomous/auto.cc
@@ -117,10 +117,10 @@
control_loops::drivetrain_queue.status.FetchAnother();
double left_error = ::std::abs(
left_initial_position -
- control_loops::drivetrain_queue.status->filtered_left_position);
+ control_loops::drivetrain_queue.status->estimated_left_position);
double right_error = ::std::abs(
right_initial_position -
- control_loops::drivetrain_queue.status->filtered_right_position);
+ control_loops::drivetrain_queue.status->estimated_right_position);
const double kPositionThreshold = 0.05 + distance;
if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
LOG(INFO, "At the goal\n");
@@ -234,9 +234,9 @@
void InitializeEncoders() {
control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain_queue.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->estimated_left_position;
right_initial_position =
- control_loops::drivetrain_queue.status->filtered_right_position;
+ control_loops::drivetrain_queue.status->estimated_right_position;
}
void WaitForClawZero() {
diff --git a/y2015/control_loops/drivetrain/drivetrain.cc b/y2015/control_loops/drivetrain/drivetrain.cc
index bc3fa21..b5eb83a 100644
--- a/y2015/control_loops/drivetrain/drivetrain.cc
+++ b/y2015/control_loops/drivetrain/drivetrain.cc
@@ -752,11 +752,11 @@
// set the output status of the control loop state
if (status) {
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->estimated_left_position = dt_closedloop.GetEstimatedLeftEncoder();
+ status->estimated_right_position = dt_closedloop.GetEstimatedRightEncoder();
- status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
- status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
+ status->estimated_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+ status->estimated_right_velocity = dt_closedloop.loop().X_hat(3, 0);
status->output_was_capped = dt_closedloop.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
diff --git a/y2015/control_loops/drivetrain/drivetrain.q b/y2015/control_loops/drivetrain/drivetrain.q
index 824688c..3edd33f 100644
--- a/y2015/control_loops/drivetrain/drivetrain.q
+++ b/y2015/control_loops/drivetrain/drivetrain.q
@@ -97,13 +97,13 @@
// Estimated speed of the center of the robot in m/s (positive forwards).
double robot_speed;
// Estimated relative position of the left side in meters.
- double filtered_left_position;
+ double estimated_left_position;
// Estimated relative position of the right side in meters.
- double filtered_right_position;
+ double estimated_right_position;
// Estimated velocity of the left side in m/s.
- double filtered_left_velocity;
+ double estimated_left_velocity;
// Estimated velocity of the right side in m/s.
- double filtered_right_velocity;
+ double estimated_right_velocity;
// The voltage we wanted to send to the left side last cycle.
double uncapped_left_voltage;
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 0f9eedd..32ce93f 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -60,7 +60,7 @@
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
- (status.filtered_left_position + status.filtered_right_position -
+ (status.estimated_left_position + status.estimated_right_position -
params.left_initial_position - params.right_initial_position) /
2.0);
} else {
@@ -142,10 +142,10 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- drivetrain_queue.status->filtered_left_position -
+ drivetrain_queue.status->estimated_left_position -
(left_goal_state(0, 0) + params.left_initial_position));
const double right_error = ::std::abs(
- drivetrain_queue.status->filtered_right_position -
+ drivetrain_queue.status->estimated_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error =
::std::abs(drivetrain_queue.status->robot_speed);
diff --git a/y2015_bot3/autonomous/auto.cc b/y2015_bot3/autonomous/auto.cc
index 9dd2892..71a0efc 100644
--- a/y2015_bot3/autonomous/auto.cc
+++ b/y2015_bot3/autonomous/auto.cc
@@ -67,9 +67,9 @@
void InitializeEncoders() {
control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain_queue.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->estimated_left_position;
right_initial_position =
- control_loops::drivetrain_queue.status->filtered_right_position;
+ control_loops::drivetrain_queue.status->estimated_right_position;
}
::std::unique_ptr< ::frc971::actors::DrivetrainAction> SetDriveGoal(
@@ -114,10 +114,10 @@
control_loops::drivetrain_queue.status.FetchAnother();
double left_error = ::std::abs(
left_initial_position -
- control_loops::drivetrain_queue.status->filtered_left_position);
+ control_loops::drivetrain_queue.status->estimated_left_position);
double right_error = ::std::abs(
right_initial_position -
- control_loops::drivetrain_queue.status->filtered_right_position);
+ control_loops::drivetrain_queue.status->estimated_right_position);
const double kPositionThreshold = 0.05 + distance;
if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
LOG(INFO, "At the goal\n");
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.cc b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
index 17cc97b..a3e7c24 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
@@ -739,11 +739,11 @@
// set the output status of the control loop state
if (status) {
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->estimated_left_position = dt_closedloop.GetEstimatedLeftEncoder();
+ status->estimated_right_position = dt_closedloop.GetEstimatedRightEncoder();
- status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
- status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
+ status->estimated_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+ status->estimated_right_velocity = dt_closedloop.loop().X_hat(3, 0);
status->output_was_capped = dt_closedloop.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.q b/y2015_bot3/control_loops/drivetrain/drivetrain.q
index 7ff0498..6d432c4 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.q
@@ -97,13 +97,13 @@
// Estimated speed of the center of the robot in m/s (positive forwards).
double robot_speed;
// Estimated relative position of the left side in meters.
- double filtered_left_position;
+ double estimated_left_position;
// Estimated relative position of the right side in meters.
- double filtered_right_position;
+ double estimated_right_position;
// Estimated velocity of the left side in m/s.
- double filtered_left_velocity;
+ double estimated_left_velocity;
// Estimated velocity of the right side in m/s.
- double filtered_right_velocity;
+ double estimated_right_velocity;
// The voltage we wanted to send to the left side last cycle.
double uncapped_left_voltage;
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
index 325f6e4..2c7c0e5 100644
--- a/y2016/actors/drivetrain_actor.cc
+++ b/y2016/actors/drivetrain_actor.cc
@@ -60,7 +60,7 @@
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
- (status.filtered_left_position + status.filtered_right_position -
+ (status.estimated_left_position + status.estimated_right_position -
params.left_initial_position - params.right_initial_position) /
2.0);
} else {
@@ -150,11 +150,11 @@
const double left_error =
::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->filtered_left_position -
+ ->estimated_left_position -
(left_goal_state(0, 0) + params.left_initial_position));
const double right_error =
::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->filtered_right_position -
+ ->estimated_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error = ::std::abs(
::frc971::control_loops::drivetrain_queue.status->robot_speed);
diff --git a/y2016/autonomous/auto.cc b/y2016/autonomous/auto.cc
index b315223..2b1c8d2 100644
--- a/y2016/autonomous/auto.cc
+++ b/y2016/autonomous/auto.cc
@@ -95,9 +95,9 @@
void InitializeEncoders() {
::frc971::control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position;
+ ::frc971::control_loops::drivetrain_queue.status->estimated_left_position;
right_initial_position =
- ::frc971::control_loops::drivetrain_queue.status->filtered_right_position;
+ ::frc971::control_loops::drivetrain_queue.status->estimated_right_position;
}
void HandleAuto() {
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 9c678bf..489a36e 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -7,8 +7,7 @@
int main() {
::aos::Init();
- DrivetrainLoop drivetrain =
- DrivetrainLoop(::y2016::control_loops::GetDrivetrainConfig());
+ DrivetrainLoop drivetrain(::y2016::control_loops::GetDrivetrainConfig());
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index 47787d4..3701e57 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -14,8 +14,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-#TODO(constants): All of the constants need to be updated for 2016.
-
class CIM(control_loop.ControlLoop):
def __init__(self):
super(CIM, self).__init__("CIM")
@@ -133,8 +131,13 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- q_pos = 0.12
- q_vel = 1.0
+ if left_low or right_low:
+ q_pos = 0.12
+ q_vel = 1.0
+ else:
+ q_pos = 0.14
+ q_vel = 0.95
+
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
[0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
@@ -144,9 +147,9 @@
[0.0, (1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- glog.debug('DT K %s', name)
- glog.debug(str(self.K))
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
self.hlp = 0.3
self.llp = 0.4
@@ -154,6 +157,7 @@
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
self.InitializeState()
@@ -170,6 +174,13 @@
#
# [left position, left velocity, right position, right velocity,
# left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
@@ -214,8 +225,26 @@
self.L = self.A * self.KalmanGain
- # We need a nothing controller for the autogen code to be happy.
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+ self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+ self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+ self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+ self.InitializeState()
def main(argv):
@@ -223,7 +252,7 @@
glog.init()
# Simulate the response of the system to a step input.
- drivetrain = Drivetrain()
+ drivetrain = Drivetrain(left_low=False, right_low=False)
simulated_left = []
simulated_right = []
for _ in xrange(100):
@@ -234,24 +263,33 @@
if FLAGS.plot:
pylab.plot(range(100), simulated_left)
pylab.plot(range(100), simulated_right)
+ pylab.suptitle('Acceleration Test')
pylab.show()
# Simulate forwards motion.
- drivetrain = Drivetrain()
+ drivetrain = Drivetrain(left_low=False, right_low=False)
close_loop_left = []
close_loop_right = []
+ left_power = []
+ right_power = []
R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
- for _ in xrange(100):
+ for _ in xrange(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
drivetrain.U_min, drivetrain.U_max)
drivetrain.UpdateObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
if FLAGS.plot:
- pylab.plot(range(100), close_loop_left)
- pylab.plot(range(100), close_loop_right)
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, label='right power')
+ pylab.suptitle('Linear Move')
+ pylab.legend()
pylab.show()
# Try turning in place
@@ -270,6 +308,7 @@
if FLAGS.plot:
pylab.plot(range(100), close_loop_left)
pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Angular Move')
pylab.show()
# Try turning just one side.
@@ -288,6 +327,7 @@
if FLAGS.plot:
pylab.plot(range(100), close_loop_left)
pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Pivot')
pylab.show()
# Write the generated constants out to a file.