Make a mutual drivetrain between robots.
Change-Id: I23cc9634d9af5d0482dc5a5501dccc064b7b53d3
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..369de78
--- /dev/null
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -0,0 +1,121 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+cc_binary(
+ name = 'replay_drivetrain',
+ srcs = [
+ 'replay_drivetrain.cc',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ '//aos/common/controls:replay_control_loop',
+ '//aos/linux_code:init',
+ '//frc971/queues:gyro',
+ ],
+)
+
+queue_library(
+ name = 'drivetrain_queue',
+ srcs = [
+ 'drivetrain.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_config',
+ hdrs = [
+ 'drivetrain_config.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971:shifter_hall_effect',
+ ],
+)
+
+cc_library(
+ name = 'ssdrivetrain',
+ srcs = [
+ 'ssdrivetrain.cc',
+ ],
+ hdrs = [
+ 'ssdrivetrain.h',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ ':drivetrain_config',
+ '//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',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain',
+ srcs = [
+ 'polydrivetrain.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain.h',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ ':drivetrain_config',
+ '//aos/common/controls:polytope',
+ '//aos/common:math',
+ '//aos/common/messages:robot_state',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:coerce_goal',
+ '//aos/common/util:log_interval',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_lib',
+ srcs = [
+ 'drivetrain.cc',
+ ],
+ hdrs = [
+ 'drivetrain.h',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ ':polydrivetrain',
+ ':ssdrivetrain',
+ '//aos/common/controls:control_loop',
+ '//frc971/queues:gyro',
+ '//aos/common/util:log_interval',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ ],
+)
+
+cc_test(
+ name = 'drivetrain_lib_test',
+ srcs = [
+ 'drivetrain_lib_test.cc',
+ ],
+ deps = [
+ '//aos/testing:googletest',
+ ':drivetrain_queue',
+ ':drivetrain_lib',
+ ':drivetrain_config',
+ '//aos/common/controls:control_loop_test',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/queues:gyro',
+ '//aos/common:queues',
+ '//y2014:constants',
+ '//y2014/control_loops/drivetrain:polydrivetrain_plants',
+ ],
+)
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
similarity index 73%
rename from y2014/control_loops/drivetrain/drivetrain.cc
rename to frc971/control_loops/drivetrain/drivetrain.cc
index 5717f36..ca395cb 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -1,4 +1,4 @@
-#include "y2014/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
#include <stdio.h>
#include <sched.h>
@@ -10,39 +10,34 @@
#include "aos/common/logging/queue_logging.h"
#include "aos/common/logging/matrix_logging.h"
-#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain.h"
-#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
-// A consistent way to mark code that goes away without shifters. It's still
-// here because we will have shifters again in the future.
-#define HAVE_SHIFTERS 1
-
using frc971::sensors::gyro_reading;
-namespace y2014 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
DrivetrainLoop::DrivetrainLoop(
- ::y2014::control_loops::DrivetrainQueue *my_drivetrain)
- : aos::controls::ControlLoop<::y2014::control_loops::DrivetrainQueue>(
+ const DrivetrainConfig &dt_config,
+ ::frc971::control_loops::DrivetrainQueue *my_drivetrain)
+ : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
my_drivetrain),
- kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()),
- dt_openloop_(&kf_) {
+ dt_config_(dt_config),
+ kf_(dt_config_.make_kf_drivetrain_loop()) {
::aos::controls::HPolytope<0>::Init();
}
void DrivetrainLoop::RunIteration(
- const ::y2014::control_loops::DrivetrainQueue::Goal *goal,
- const ::y2014::control_loops::DrivetrainQueue::Position *position,
- ::y2014::control_loops::DrivetrainQueue::Output *output,
- ::y2014::control_loops::DrivetrainQueue::Status *status) {
+ 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_);
@@ -56,8 +51,9 @@
Eigen::Matrix<double, 3, 1> Y;
Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
kf_.Correct(Y);
- integrated_kf_heading_ +=
- kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
+ integrated_kf_heading_ += dt_config_.dt *
+ (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) /
+ (dt_config_.robot_radius * 2.0);
}
bool control_loop_driving = false;
@@ -65,9 +61,7 @@
double wheel = goal->steering;
double throttle = goal->throttle;
bool quickturn = goal->quickturn;
-#if HAVE_SHIFTERS
bool highgear = goal->highgear;
-#endif
control_loop_driving = goal->control_loop_driving;
double left_goal = goal->left_goal;
@@ -75,11 +69,7 @@
dt_closedloop_.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
goal->right_velocity_goal);
-#if HAVE_SHIFTERS
dt_openloop_.SetGoal(wheel, throttle, quickturn, highgear);
-#else
- dt_openloop_.SetGoal(wheel, throttle, quickturn, false);
-#endif
}
if (!bad_pos) {
@@ -123,7 +113,6 @@
status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
}
-
double left_voltage = 0.0;
double right_voltage = 0.0;
if (output) {
@@ -154,4 +143,4 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2014
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
new file mode 100644
index 0000000..22a6c32
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -0,0 +1,58 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
+
+#include "Eigen/Dense"
+
+#include "aos/common/controls/polytope.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/controls/polytope.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.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 {
+namespace drivetrain {
+
+class DrivetrainLoop : public aos::controls::ControlLoop<
+ ::frc971::control_loops::DrivetrainQueue> {
+ public:
+ // Constructs a control loop which can take a Drivetrain or defaults to the
+ // drivetrain at frc971::control_loops::drivetrain
+ explicit DrivetrainLoop(const DrivetrainConfig &dt_config,
+ ::frc971::control_loops::DrivetrainQueue *my_drivetrain =
+ &::frc971::control_loops::drivetrain_queue);
+
+ protected:
+ // Executes one cycle of the control loop.
+ virtual void 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);
+
+ 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;
+
+ const DrivetrainConfig dt_config_;
+
+ PolyDrivetrain dt_openloop_{dt_config_};
+ DrivetrainMotorsSS dt_closedloop_{dt_config_};
+ StateFeedbackLoop<7, 2, 3> kf_;
+
+ double last_left_voltage_ = 0;
+ double last_right_voltage_ = 0;
+
+ double integrated_kf_heading_ = 0;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/y2014/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
similarity index 99%
rename from y2014/control_loops/drivetrain/drivetrain.q
rename to frc971/control_loops/drivetrain/drivetrain.q
index 96bd817..b4a71ec 100644
--- a/y2014/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -1,4 +1,4 @@
-package y2014.control_loops;
+package frc971.control_loops;
import "aos/common/controls/control_loops.q";
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
new file mode 100644
index 0000000..07fbe93
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -0,0 +1,54 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
+
+#include <functional>
+
+#include "frc971/shifter_hall_effect.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+enum class ShifterType : int32_t {
+ HALL_EFFECT_SHIFTER = 0, // Detect when inbetween gears.
+ SIMPLE_SHIFTER = 1, // Switch gears without speedmatch logic.
+};
+
+struct DrivetrainConfig {
+ // Shifting method we are using.
+ ShifterType shifter_type;
+
+ // Polydrivetrain functions returning various controller loops with plants.
+ ::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
+ ::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
+ ::std::function<StateFeedbackLoop<7, 2, 3>()> make_kf_drivetrain_loop;
+
+ double dt; // Control loop time step.
+ double stall_torque; // Stall torque in N m.
+ double stall_current; // Stall current in amps.
+ double free_speed_rpm; // Free speed in rpm.
+ double free_current; // Free current in amps.
+ double j; // CIM moment of inertia in kg m^2.
+ double mass; // Mass of the robot.
+ double robot_radius; // Robot radius, in meters.
+ double wheel_radius; // Wheel radius, in meters.
+ double r; // Motor resistance.
+ double v; // Motor velocity constant.
+ double t; // Torque constant.
+
+ double turn_width; // Robot turn width, in meters.
+ // Gear ratios, from encoder shaft to transmission output.
+ double high_gear_ratio;
+ double low_gear_ratio;
+
+ // Hall effect constants. Unused if not applicable to shifter type.
+ constants::ShifterHallEffect left_drive;
+ constants::ShifterHallEffect right_drive;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
diff --git a/y2014/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
similarity index 74%
rename from y2014/control_loops/drivetrain/drivetrain_lib_test.cc
rename to frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index adfc519..9db00b5 100644
--- a/y2014/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -7,21 +7,53 @@
#include "aos/common/controls/polytope.h"
#include "aos/common/controls/control_loop_test.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/drivetrain/drivetrain.h"
+#include "y2014/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.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 "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "frc971/queues/gyro.q.h"
-
-namespace y2014 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
namespace testing {
using ::y2014::control_loops::drivetrain::MakeDrivetrainPlant;
+// TODO(Comran): Make one that doesn't depend on the actual values for a
+// specific robot.
+const DrivetrainConfig kDrivetrainConfig {
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+
+ ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop,
+
+ ::y2014::control_loops::drivetrain::kDt,
+ ::y2014::control_loops::drivetrain::kStallTorque,
+ ::y2014::control_loops::drivetrain::kStallCurrent,
+ ::y2014::control_loops::drivetrain::kFreeSpeedRPM,
+ ::y2014::control_loops::drivetrain::kFreeCurrent,
+ ::y2014::control_loops::drivetrain::kJ,
+ ::y2014::control_loops::drivetrain::kMass,
+ ::y2014::control_loops::drivetrain::kRobotRadius,
+ ::y2014::control_loops::drivetrain::kWheelRadius,
+ ::y2014::control_loops::drivetrain::kR,
+ ::y2014::control_loops::drivetrain::kV,
+ ::y2014::control_loops::drivetrain::kT,
+
+ ::y2014::constants::GetValuesForTeam(971).turn_width,
+ ::y2014::constants::GetValuesForTeam(971).high_gear_ratio,
+ ::y2014::constants::GetValuesForTeam(971).low_gear_ratio,
+ ::y2014::constants::GetValuesForTeam(971).left_drive,
+ ::y2014::constants::GetValuesForTeam(971).right_drive
+};
+
class Environment : public ::testing::Environment {
public:
virtual ~Environment() {}
@@ -51,11 +83,11 @@
DrivetrainSimulation()
: drivetrain_plant_(
new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
- my_drivetrain_queue_(".y2014.control_loops.drivetrain",
- 0x8a8dde77, ".y2014.control_loops.drivetrain.goal",
- ".y2014.control_loops.drivetrain.position",
- ".y2014.control_loops.drivetrain.output",
- ".y2014.control_loops.drivetrain.status") {
+ 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") {
Reinitialize();
}
@@ -78,7 +110,7 @@
const double left_encoder = GetLeftPosition();
const double right_encoder = GetRightPosition();
- ::aos::ScopedMessagePtr<::y2014::control_loops::DrivetrainQueue::Position>
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
position = my_drivetrain_queue_.position.MakeMessage();
position->left_encoder = left_encoder;
position->right_encoder = right_encoder;
@@ -97,7 +129,7 @@
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
private:
- ::y2014::control_loops::DrivetrainQueue my_drivetrain_queue_;
+ ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
double last_left_position_;
double last_right_position_;
};
@@ -107,19 +139,19 @@
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory that
// is no longer valid.
- ::y2014::control_loops::DrivetrainQueue my_drivetrain_queue_;
+ ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
- DrivetrainTest() : my_drivetrain_queue_(".y2014.control_loops.drivetrain",
+ DrivetrainTest() : my_drivetrain_queue_(".frc971.control_loops.drivetrain",
0x8a8dde77,
- ".y2014.control_loops.drivetrain.goal",
- ".y2014.control_loops.drivetrain.position",
- ".y2014.control_loops.drivetrain.output",
- ".y2014.control_loops.drivetrain.status"),
- drivetrain_motor_(&my_drivetrain_queue_),
+ ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status"),
+ drivetrain_motor_(kDrivetrainConfig, &my_drivetrain_queue_),
drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
}
@@ -297,4 +329,4 @@
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2014
+} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
similarity index 71%
rename from y2014/control_loops/drivetrain/polydrivetrain.cc
rename to frc971/control_loops/drivetrain/polydrivetrain.cc
index 014a1ae..dd9354e 100644
--- a/y2014/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,4 +1,4 @@
-#include "y2014/control_loops/drivetrain/polydrivetrain.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "aos/common/logging/logging.h"
#include "aos/common/controls/polytope.h"
@@ -9,22 +9,19 @@
#include "aos/common/messages/robot_state.q.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#define HAVE_SHIFTERS 1
-
-namespace y2014 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-using ::y2014::control_loops::GearLogging;
-using ::y2014::control_loops::CIMLogging;
+using ::frc971::control_loops::GearLogging;
+using ::frc971::control_loops::CIMLogging;
using ::frc971::control_loops::CoerceGoal;
-PolyDrivetrain::PolyDrivetrain(StateFeedbackLoop<7, 2, 3> *kf)
- : kf_(kf),
+PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config)
+ : kf_(dt_config.make_kf_drivetrain_loop()),
U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
@@ -33,17 +30,17 @@
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]]*/).finished()),
- loop_(new StateFeedbackLoop<2, 2, 2>(
- constants::GetValues().make_v_drivetrain_loop())),
+ loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
ttrust_(1.1),
wheel_(0.0),
throttle_(0.0),
quickturn_(false),
stale_count_(0),
- position_time_delta_(kDt),
+ position_time_delta_(dt_config.dt),
left_gear_(LOW),
right_gear_(LOW),
- counter_(0) {
+ counter_(0),
+ dt_config_(dt_config) {
last_position_.Zero();
position_.Zero();
}
@@ -55,19 +52,22 @@
(hall_effect.clear_high + hall_effect.clear_low) / 2.0;
if (shifter_position > avg_hall_effect) {
- return velocity / constants::GetValues().high_gear_ratio / kWheelRadius;
+ return velocity / dt_config_.high_gear_ratio / dt_config_.wheel_radius;
} else {
- return velocity / constants::GetValues().low_gear_ratio / kWheelRadius;
+ return velocity / dt_config_.low_gear_ratio / dt_config_.wheel_radius;
}
}
-PolyDrivetrain::Gear PolyDrivetrain::UpdateSingleGear(
- Gear requested_gear, Gear current_gear) {
+PolyDrivetrain::Gear PolyDrivetrain::UpdateSingleGear(Gear requested_gear,
+ Gear current_gear) {
const Gear shift_up =
- constants::GetValues().clutch_transmission ? HIGH : SHIFTING_UP;
+ (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
+ ? SHIFTING_UP
+ : HIGH;
const Gear shift_down =
- constants::GetValues().clutch_transmission ? LOW : SHIFTING_DOWN;
-
+ (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER)
+ ? SHIFTING_DOWN
+ : LOW;
if (current_gear != requested_gear) {
if (IsInGear(current_gear)) {
if (requested_gear == HIGH) {
@@ -101,6 +101,16 @@
// 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);
quickturn_ = quickturn;
@@ -118,24 +128,25 @@
}
void PolyDrivetrain::SetPosition(
- const ::y2014::control_loops::DrivetrainQueue::Position *position) {
+ const ::frc971::control_loops::DrivetrainQueue::Position *position) {
if (position == NULL) {
++stale_count_;
} else {
last_position_ = position_;
position_ = *position;
- position_time_delta_ = (stale_count_ + 1) * kDt;
+ position_time_delta_ = (stale_count_ + 1) * dt_config_.dt;
stale_count_ = 0;
}
- if (position) {
- const auto &values = constants::GetValues();
+ if (dt_config_.shifter_type == ShifterType::HALL_EFFECT_SHIFTER && position) {
GearLogging gear_logging;
// Switch to the correct controller.
const double left_middle_shifter_position =
- (values.left_drive.clear_high + values.left_drive.clear_low) / 2.0;
+ (dt_config_.left_drive.clear_high + dt_config_.left_drive.clear_low) /
+ 2.0;
const double right_middle_shifter_position =
- (values.right_drive.clear_high + values.right_drive.clear_low) / 2.0;
+ (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) {
@@ -162,19 +173,19 @@
}
}
- if (position->left_shifter_position > values.left_drive.clear_high &&
+ if (position->left_shifter_position > dt_config_.left_drive.clear_high &&
left_gear_ == SHIFTING_UP) {
left_gear_ = HIGH;
}
- if (position->left_shifter_position < values.left_drive.clear_low &&
+ if (position->left_shifter_position < dt_config_.left_drive.clear_low &&
left_gear_ == SHIFTING_DOWN) {
left_gear_ = LOW;
}
- if (position->right_shifter_position > values.right_drive.clear_high &&
+ if (position->right_shifter_position > dt_config_.right_drive.clear_high &&
right_gear_ == SHIFTING_UP) {
right_gear_ = HIGH;
}
- if (position->right_shifter_position < values.right_drive.clear_low &&
+ if (position->right_shifter_position < dt_config_.right_drive.clear_low &&
right_gear_ == SHIFTING_DOWN) {
right_gear_ = LOW;
}
@@ -233,48 +244,12 @@
}
void PolyDrivetrain::Update() {
- loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
- loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
+ loop_->mutable_X_hat()(0, 0) = kf_.X_hat()(1, 0);
+ loop_->mutable_X_hat()(1, 0) = kf_.X_hat()(3, 0);
- const auto &values = constants::GetValues();
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
- const double current_left_velocity =
- (position_.left_encoder - last_position_.left_encoder) /
- position_time_delta_;
- const double current_right_velocity =
- (position_.right_encoder - last_position_.right_encoder) /
- position_time_delta_;
- const double left_motor_speed =
- MotorSpeed(values.left_drive, position_.left_shifter_position,
- current_left_velocity);
- const double right_motor_speed =
- MotorSpeed(values.right_drive, position_.right_shifter_position,
- current_right_velocity);
-
- {
- CIMLogging logging;
-
- // Reset the CIM model to the current conditions to be ready for when we
- // shift.
- if (IsInGear(left_gear_)) {
- logging.left_in_gear = true;
- } else {
- logging.left_in_gear = false;
- }
- logging.left_motor_speed = left_motor_speed;
- logging.left_velocity = current_left_velocity;
- if (IsInGear(right_gear_)) {
- logging.right_in_gear = true;
- } else {
- logging.right_in_gear = false;
- }
- logging.right_motor_speed = right_motor_speed;
- logging.right_velocity = current_right_velocity;
-
- LOG_STRUCT(DEBUG, "currently", logging);
- }
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
// FF * X = U (steady state)
@@ -328,6 +303,42 @@
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
}
} else {
+ const double current_left_velocity =
+ (position_.left_encoder - last_position_.left_encoder) /
+ position_time_delta_;
+ const double current_right_velocity =
+ (position_.right_encoder - last_position_.right_encoder) /
+ position_time_delta_;
+ const double left_motor_speed =
+ MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
+ current_left_velocity);
+ const double right_motor_speed =
+ MotorSpeed(dt_config_.right_drive, position_.right_shifter_position,
+ current_right_velocity);
+
+ {
+ CIMLogging logging;
+
+ // Reset the CIM model to the current conditions to be ready for when we
+ // shift.
+ if (IsInGear(left_gear_)) {
+ logging.left_in_gear = true;
+ } else {
+ logging.left_in_gear = false;
+ }
+ logging.left_motor_speed = left_motor_speed;
+ logging.left_velocity = current_left_velocity;
+ if (IsInGear(right_gear_)) {
+ logging.right_in_gear = true;
+ } else {
+ logging.right_in_gear = false;
+ }
+ logging.right_motor_speed = right_motor_speed;
+ logging.right_velocity = current_right_velocity;
+
+ LOG_STRUCT(DEBUG, "currently", logging);
+ }
+
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
::Eigen::Matrix<double, 1, 1> R_right;
@@ -338,16 +349,17 @@
(static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
loop_->mutable_U(0, 0) = ::aos::Clip(
- (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle), -12.0, 12.0);
- loop_->mutable_U(1, 0) =
- ::aos::Clip((R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
- -12.0, 12.0);
+ (R_left / dt_config_.v)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->mutable_U(1, 0) = ::aos::Clip(
+ (R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
loop_->mutable_U() *= 12.0 / ::aos::robot_state->voltage_battery;
}
}
void PolyDrivetrain::SendMotors(
- ::y2014::control_loops::DrivetrainQueue::Output *output) {
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -356,15 +368,6 @@
}
}
-constexpr double PolyDrivetrain::kStallTorque;
-constexpr double PolyDrivetrain::kStallCurrent;
-constexpr double PolyDrivetrain::kFreeSpeed;
-constexpr double PolyDrivetrain::kFreeCurrent;
-constexpr double PolyDrivetrain::kWheelRadius;
-constexpr double PolyDrivetrain::kR;
-constexpr double PolyDrivetrain::Kv;
-constexpr double PolyDrivetrain::Kt;
-
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2014
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
new file mode 100644
index 0000000..1ba7eee
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -0,0 +1,75 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
+
+#include "aos/common/controls/polytope.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"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class PolyDrivetrain {
+ public:
+ enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
+
+ PolyDrivetrain(const DrivetrainConfig &dt_config);
+
+ 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);
+
+ // 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 SetPosition(
+ const ::frc971::control_loops::DrivetrainQueue::Position *position);
+
+ double FilterVelocity(double throttle);
+
+ double MaxVelocity();
+
+ void Update();
+
+ void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
+
+ private:
+ StateFeedbackLoop<7, 2, 3> kf_;
+
+ const ::aos::controls::HPolytope<2> U_Poly_;
+
+ ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
+
+ const double ttrust_;
+ 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_;
+ DrivetrainConfig dt_config_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
diff --git a/y2014/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
similarity index 64%
rename from y2014/control_loops/drivetrain/replay_drivetrain.cc
rename to frc971/control_loops/drivetrain/replay_drivetrain.cc
index b0ac33f..c535607 100644
--- a/y2014/control_loops/drivetrain/replay_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/replay_drivetrain.cc
@@ -1,7 +1,7 @@
#include "aos/common/controls/replay_control_loop.h"
#include "aos/linux_code/init.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/gyro.q.h"
// Reads one or more log files and sends out all the queue messages (in the
@@ -17,8 +17,8 @@
{
::aos::controls::ControlLoopReplayer<
- ::y2014::control_loops::DrivetrainQueue>
- replayer(&::y2014::control_loops::drivetrain_queue, "drivetrain");
+ ::frc971::control_loops::DrivetrainQueue>
+ replayer(&::frc971::control_loops::drivetrain_queue, "drivetrain");
replayer.AddDirectQueueSender("wpilib_interface.Gyro", "sending",
::frc971::sensors::gyro_reading);
@@ -27,10 +27,10 @@
}
}
::frc971::sensors::gyro_reading.Clear();
- ::y2014::control_loops::drivetrain_queue.goal.Clear();
- ::y2014::control_loops::drivetrain_queue.status.Clear();
- ::y2014::control_loops::drivetrain_queue.position.Clear();
- ::y2014::control_loops::drivetrain_queue.output.Clear();
+ ::frc971::control_loops::drivetrain_queue.goal.Clear();
+ ::frc971::control_loops::drivetrain_queue.status.Clear();
+ ::frc971::control_loops::drivetrain_queue.position.Clear();
+ ::frc971::control_loops::drivetrain_queue.output.Clear();
::aos::Cleanup();
}
diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
similarity index 90%
rename from y2014/control_loops/drivetrain/ssdrivetrain.cc
rename to frc971/control_loops/drivetrain/ssdrivetrain.cc
index 45ab4eb..049b576 100644
--- a/y2014/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -1,4 +1,4 @@
-#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "aos/common/controls/polytope.h"
#include "aos/common/commonmath.h"
@@ -6,10 +6,10 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-namespace y2014 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -113,15 +113,16 @@
}
}
-DrivetrainMotorsSS::DrivetrainMotorsSS()
- : loop_(new LimitedDrivetrainLoop(
- constants::GetValues().make_drivetrain_loop())),
+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) {
+ raw_right_(0.0),
+ dt_config_(dt_config) {
// High gear on both.
loop_->set_controller_index(3);
}
@@ -142,7 +143,7 @@
void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
const double offset =
- (right - left - gyro * constants::GetValues().turn_width) / 2.0;
+ (right - left - gyro * dt_config_.turn_width) / 2.0;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);
@@ -173,7 +174,7 @@
}
void DrivetrainMotorsSS::SendMotors(
- ::y2014::control_loops::DrivetrainQueue::Output *output) const {
+ ::frc971::control_loops::DrivetrainQueue::Output *output) const {
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -184,4 +185,4 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2014
+} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
similarity index 78%
rename from y2014/control_loops/drivetrain/ssdrivetrain.h
rename to frc971/control_loops/drivetrain/ssdrivetrain.h
index 1964ac5..1ed3eb7 100644
--- a/y2014/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -1,5 +1,5 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
+#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"
@@ -7,10 +7,10 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-namespace y2014 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -32,7 +32,7 @@
bool output_was_capped_ = false;;
};
- DrivetrainMotorsSS();
+ DrivetrainMotorsSS(const DrivetrainConfig &dt_config);
void SetGoal(double left, double left_velocity, double right,
double right_velocity);
@@ -63,7 +63,7 @@
}
void SendMotors(
- ::y2014::control_loops::DrivetrainQueue::Output *output) const;
+ ::frc971::control_loops::DrivetrainQueue::Output *output) const;
const LimitedDrivetrainLoop &loop() const { return *loop_; }
@@ -76,10 +76,12 @@
double right_goal_;
double raw_left_;
double raw_right_;
+
+ const DrivetrainConfig dt_config_;
};
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2014
+} // namespace frc971
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
diff --git a/y2014/BUILD b/y2014/BUILD
index 6561047..2938aa6 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -33,7 +33,7 @@
'//aos/common:time',
'//aos/common/util:log_interval',
'//aos/common/actions:action_lib',
- '//y2014/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/queues:gyro',
'//frc971/autonomous:auto_queue',
'//y2014/control_loops/claw:claw_queue',
diff --git a/y2014/actors/BUILD b/y2014/actors/BUILD
index 6fea9eb..ef94367 100644
--- a/y2014/actors/BUILD
+++ b/y2014/actors/BUILD
@@ -35,7 +35,7 @@
'//aos/common/logging',
'//y2014/control_loops/shooter:shooter_queue',
'//y2014/control_loops/claw:claw_queue',
- '//y2014/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
'//y2014:constants',
],
)
@@ -81,7 +81,7 @@
'//aos/common/logging:queue_logging',
'//third_party/eigen',
'//aos/common/util:trapezoid_profile',
- '//y2014/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/control_loops:state_feedback_loop',
],
)
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 9f94ff4..4cb69ca 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -13,7 +13,7 @@
#include "y2014/actors/drivetrain_actor.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace y2014 {
namespace actors {
@@ -50,9 +50,9 @@
while (true) {
::aos::time::PhasedLoopXMS(5, 2500);
- control_loops::drivetrain_queue.status.FetchLatest();
- if (control_loops::drivetrain_queue.status.get()) {
- const auto& status = *control_loops::drivetrain_queue.status;
+ ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
+ if (::frc971::control_loops::drivetrain_queue.status.get()) {
+ const auto& status = *::frc971::control_loops::drivetrain_queue.status;
if (::std::abs(status.uncapped_left_voltage -
status.uncapped_right_voltage) > 24) {
LOG(DEBUG, "spinning in place\n");
@@ -119,7 +119,7 @@
LOG(DEBUG, "Driving left to %f, right to %f\n",
left_goal_state(0, 0) + params.left_initial_position,
right_goal_state(0, 0) + params.right_initial_position);
- control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(true)
.left_goal(left_goal_state(0, 0) + params.left_initial_position)
@@ -129,11 +129,11 @@
.Send();
}
if (ShouldCancel()) return true;
- control_loops::drivetrain_queue.status.FetchLatest();
- while (!control_loops::drivetrain_queue.status.get()) {
+ ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
+ while (!::frc971::control_loops::drivetrain_queue.status.get()) {
LOG(WARNING,
"No previous drivetrain status packet, trying to fetch again\n");
- control_loops::drivetrain_queue.status.FetchNextBlocking();
+ ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
if (ShouldCancel()) return true;
}
while (true) {
@@ -141,13 +141,13 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- control_loops::drivetrain_queue.status->filtered_left_position -
+ ::frc971::control_loops::drivetrain_queue.status->filtered_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 -
+ ::frc971::control_loops::drivetrain_queue.status->filtered_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error =
- ::std::abs(control_loops::drivetrain_queue.status->robot_speed);
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
velocity_error < 0.2) {
break;
@@ -155,7 +155,7 @@
LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
velocity_error);
}
- control_loops::drivetrain_queue.status.FetchNextBlocking();
+ ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
}
LOG(INFO, "Done moving\n");
return true;
diff --git a/y2014/actors/shoot_actor.cc b/y2014/actors/shoot_actor.cc
index a70d692..743a4c3 100644
--- a/y2014/actors/shoot_actor.cc
+++ b/y2014/actors/shoot_actor.cc
@@ -7,7 +7,7 @@
#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/control_loops/claw/claw.q.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace y2014 {
namespace actors {
diff --git a/y2014/autonomous/BUILD b/y2014/autonomous/BUILD
index 4a4c842..984b110 100644
--- a/y2014/autonomous/BUILD
+++ b/y2014/autonomous/BUILD
@@ -11,7 +11,7 @@
deps = [
'//frc971/autonomous:auto_queue',
'//aos/common/controls:control_loop',
- '//y2014/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
'//y2014/control_loops/shooter:shooter_queue',
'//y2014/control_loops/claw:claw_queue',
'//y2014:constants',
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index 899d071..e954637 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -11,7 +11,7 @@
#include "frc971/autonomous/auto.q.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/control_loops/claw/claw.q.h"
#include "y2014/actors/shoot_actor.h"
@@ -41,7 +41,7 @@
void StopDrivetrain() {
LOG(INFO, "Stopping the drivetrain\n");
- control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(true)
.left_goal(left_initial_position)
@@ -54,7 +54,7 @@
void ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
- control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
.highgear(true)
.steering(0.0)
@@ -86,7 +86,7 @@
theta * constants::GetValues().turn_width / 2.0);
double right_goal = (right_initial_position + distance +
theta * constants::GetValues().turn_width / 2.0);
- control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(true)
.left_goal(left_goal)
@@ -159,13 +159,13 @@
void WaitUntilNear(double distance) {
while (true) {
if (ShouldExitAuto()) return;
- control_loops::drivetrain_queue.status.FetchAnother();
+ ::frc971::control_loops::drivetrain_queue.status.FetchAnother();
double left_error = ::std::abs(
left_initial_position -
- control_loops::drivetrain_queue.status->filtered_left_position);
+ ::frc971::control_loops::drivetrain_queue.status->filtered_left_position);
double right_error = ::std::abs(
right_initial_position -
- control_loops::drivetrain_queue.status->filtered_right_position);
+ ::frc971::control_loops::drivetrain_queue.status->filtered_right_position);
const double kPositionThreshold = 0.05 + distance;
if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
LOG(INFO, "At the goal\n");
@@ -211,11 +211,11 @@
}
void InitializeEncoders() {
- control_loops::drivetrain_queue.status.FetchAnother();
+ ::frc971::control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain_queue.status->filtered_left_position;
+ ::frc971::control_loops::drivetrain_queue.status->filtered_left_position;
right_initial_position =
- control_loops::drivetrain_queue.status->filtered_right_position;
+ ::frc971::control_loops::drivetrain_queue.status->filtered_right_position;
}
void WaitUntilClawDone() {
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 54d6581..9d7edb6 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -2,29 +2,6 @@
load('/aos/build/queues', 'queue_library')
-cc_binary(
- name = 'replay_drivetrain',
- srcs = [
- 'replay_drivetrain.cc',
- ],
- deps = [
- ':drivetrain_queue',
- '//aos/common/controls:replay_control_loop',
- '//aos/linux_code:init',
- '//frc971/queues:gyro',
- ],
-)
-
-queue_library(
- name = 'drivetrain_queue',
- srcs = [
- 'drivetrain.q',
- ],
- deps = [
- '//aos/common/controls:control_loop_queues',
- ],
-)
-
genrule(
name = 'genrule_drivetrain',
visibility = ['//visibility:private'],
@@ -73,83 +50,17 @@
)
cc_library(
- name = 'ssdrivetrain',
+ name = 'drivetrain_base',
srcs = [
- 'ssdrivetrain.cc',
+ 'drivetrain_base.cc',
],
hdrs = [
- 'ssdrivetrain.h',
+ 'drivetrain_base.h',
],
deps = [
- ':drivetrain_queue',
+ ':polydrivetrain_plants',
'//y2014:constants',
- '//aos/common/controls:polytope',
- '//aos/common:math',
- '//aos/common/messages:robot_state',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:coerce_goal',
- '//aos/common/util:log_interval',
- '//aos/common/logging:queue_logging',
- '//aos/common/logging:matrix_logging',
- ],
-)
-
-cc_library(
- name = 'polydrivetrain',
- srcs = [
- 'polydrivetrain.cc',
- ],
- hdrs = [
- 'polydrivetrain.h',
- ],
- deps = [
- ':drivetrain_queue',
- '//y2014:constants',
- '//aos/common/controls:polytope',
- '//aos/common:math',
- '//aos/common/messages:robot_state',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:coerce_goal',
- '//aos/common/util:log_interval',
- '//aos/common/logging:queue_logging',
- '//aos/common/logging:matrix_logging',
- ],
-)
-
-cc_library(
- name = 'drivetrain_lib',
- srcs = [
- 'drivetrain.cc',
- ],
- hdrs = [
- 'drivetrain.h',
- ],
- deps = [
- ':drivetrain_queue',
- ':polydrivetrain',
- ':ssdrivetrain',
- '//aos/common/controls:control_loop',
- '//y2014:constants',
- '//frc971/queues:gyro',
- '//aos/common/util:log_interval',
- '//aos/common/logging:queue_logging',
- '//aos/common/logging:matrix_logging',
- ],
-)
-
-cc_test(
- name = 'drivetrain_lib_test',
- srcs = [
- 'drivetrain_lib_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- ':drivetrain_queue',
- ':drivetrain_lib',
- '//aos/common/controls:control_loop_test',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/queues:gyro',
- '//aos/common:queues',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
],
)
@@ -159,8 +70,8 @@
'drivetrain_main.cc',
],
deps = [
+ ':drivetrain_base',
'//aos/linux_code:init',
- ':drivetrain_lib',
- ':drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_lib',
],
)
diff --git a/y2014/control_loops/drivetrain/drivetrain.h b/y2014/control_loops/drivetrain/drivetrain.h
deleted file mode 100644
index 489b4ca..0000000
--- a/y2014/control_loops/drivetrain/drivetrain.h
+++ /dev/null
@@ -1,55 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_H_
-
-#include "Eigen/Dense"
-
-#include "aos/common/controls/polytope.h"
-#include "aos/common/controls/control_loop.h"
-#include "aos/common/controls/polytope.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain.h"
-#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
-#include "aos/common/util/log_interval.h"
-
-namespace y2014 {
-namespace control_loops {
-namespace drivetrain {
-
-class DrivetrainLoop : public aos::controls::ControlLoop<
- ::y2014::control_loops::DrivetrainQueue> {
- public:
- // Constructs a control loop which can take a Drivetrain or defaults to the
- // drivetrain at y2014::control_loops::drivetrain
- explicit DrivetrainLoop(
- ::y2014::control_loops::DrivetrainQueue *my_drivetrain =
- &::y2014::control_loops::drivetrain_queue);
-
- protected:
- // Executes one cycle of the control loop.
- virtual void RunIteration(
- const ::y2014::control_loops::DrivetrainQueue::Goal *goal,
- const ::y2014::control_loops::DrivetrainQueue::Position *position,
- ::y2014::control_loops::DrivetrainQueue::Output *output,
- ::y2014::control_loops::DrivetrainQueue::Status *status);
-
- 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;
-
- StateFeedbackLoop<7, 2, 3> kf_;
- PolyDrivetrain dt_openloop_;
- DrivetrainMotorsSS dt_closedloop_;
-
- double last_left_voltage_ = 0;
- double last_right_voltage_ = 0;
-
- double integrated_kf_heading_ = 0;
-};
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2014
-
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..b4ef447
--- /dev/null
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,36 @@
+#include "y2014/control_loops/drivetrain/drivetrain_base.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2014/constants.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace y2014 {
+namespace control_loops {
+
+const DrivetrainConfig &GetDrivetrainConfig() {
+ static DrivetrainConfig kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+
+ ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop,
+
+ drivetrain::kDt, drivetrain::kStallTorque, drivetrain::kStallCurrent,
+ drivetrain::kFreeSpeedRPM, drivetrain::kFreeCurrent, drivetrain::kJ,
+ drivetrain::kMass, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
+ drivetrain::kR, drivetrain::kV, drivetrain::kT,
+
+ constants::GetValues().turn_width, constants::GetValues().high_gear_ratio,
+ constants::GetValues().low_gear_ratio,
+ constants::GetValues().left_drive, constants::GetValues().right_drive};
+
+ return kDrivetrainConfig;
+};
+
+} // namespace control_loops
+} // namespace y2014
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.h b/y2014/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..3a9d70e
--- /dev/null
+++ b/y2014/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,16 @@
+#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace y2014 {
+namespace control_loops {
+
+const DrivetrainConfig &GetDrivetrainConfig();
+
+} // namespace control_loops
+} // namespace y2014
+
+#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index cc71c69..52dafcc 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -1,10 +1,14 @@
-#include "y2014/control_loops/drivetrain/drivetrain.h"
-
#include "aos/linux_code/init.h"
+#include "y2014/control_loops/drivetrain/drivetrain_base.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
int main() {
::aos::Init();
- ::y2014::control_loops::drivetrain::DrivetrainLoop drivetrain;
+ DrivetrainLoop drivetrain =
+ DrivetrainLoop(::y2014::control_loops::GetDrivetrainConfig());
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.h b/y2014/control_loops/drivetrain/polydrivetrain.h
deleted file mode 100644
index 5cd7fff..0000000
--- a/y2014/control_loops/drivetrain/polydrivetrain.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
-
-#include "aos/common/controls/polytope.h"
-
-#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-namespace y2014 {
-namespace control_loops {
-namespace drivetrain {
-
-class PolyDrivetrain {
- public:
- enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
- // Stall Torque in N m
- static constexpr double kStallTorque = drivetrain::kStallTorque;
- // Stall Current in Amps
- static constexpr double kStallCurrent = drivetrain::kStallCurrent;
- // Free Speed in RPM. Used number from last year.
- static constexpr double kFreeSpeed = drivetrain::kFreeSpeedRPM;
- // Free Current in Amps
- static constexpr double kFreeCurrent = drivetrain::kFreeCurrent;
- static constexpr double kWheelRadius = drivetrain::kWheelRadius;
- // Resistance of the motor, divided by the number of motors per side.
- static constexpr double kR = drivetrain::kR;
- // Motor velocity constant
- static constexpr double Kv = drivetrain::kV;
-
- // Torque constant
- static constexpr double Kt = drivetrain::kT;
-
- PolyDrivetrain(StateFeedbackLoop<7, 2, 3> *kf);
-
- int controller_index() const { return loop_->controller_index(); }
-
- static 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.
- static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
- double shifter_position, double velocity);
-
- // 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.
- static Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
-
- void SetGoal(double wheel, double throttle, bool quickturn, bool highgear);
-
- void SetPosition(
- const ::y2014::control_loops::DrivetrainQueue::Position *position);
-
- double FilterVelocity(double throttle);
-
- double MaxVelocity();
-
- void Update();
-
- void SendMotors(::y2014::control_loops::DrivetrainQueue::Output *output);
-
- private:
- StateFeedbackLoop<7, 2, 3> *kf_;
- const ::aos::controls::HPolytope<2> U_Poly_;
-
- ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
-
- const double ttrust_;
- double wheel_;
- double throttle_;
- bool quickturn_;
- int stale_count_;
- double position_time_delta_;
- Gear left_gear_;
- Gear right_gear_;
- ::y2014::control_loops::DrivetrainQueue::Position last_position_;
- ::y2014::control_loops::DrivetrainQueue::Position position_;
- int counter_;
-};
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2014
-
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 87dc3f1..137856f 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -11,7 +11,7 @@
#include "aos/common/time.h"
#include "aos/common/actions/actions.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
@@ -19,7 +19,7 @@
#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/actors/shoot_actor.h"
-using ::y2014::control_loops::drivetrain_queue;
+using ::frc971::control_loops::drivetrain_queue;
using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
@@ -427,11 +427,11 @@
velocity_compensation_ = 0.0;
}
- control_loops::drivetrain_queue.status.FetchLatest();
+ ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
double goal_angle = goal_angle_;
- if (control_loops::drivetrain_queue.status.get()) {
+ if (::frc971::control_loops::drivetrain_queue.status.get()) {
goal_angle += SpeedToAngleOffset(
- control_loops::drivetrain_queue.status->robot_speed);
+ ::frc971::control_loops::drivetrain_queue.status->robot_speed);
} else {
LOG_INTERVAL(no_drivetrain_status_);
}
diff --git a/y2014/wpilib/BUILD b/y2014/wpilib/BUILD
index 38853ca..120ff49 100644
--- a/y2014/wpilib/BUILD
+++ b/y2014/wpilib/BUILD
@@ -12,7 +12,7 @@
'//aos/externals:wpilib',
'//y2014:constants',
'//y2014/queues:auto_mode',
- '//y2014/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
'//y2014/control_loops/shooter:shooter_queue',
'//y2014/control_loops/claw:claw_queue',
'//aos/common/controls:control_loop',
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 13b2e7b..6442a2a 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -32,7 +32,7 @@
#include "frc971/shifter_hall_effect.h"
-#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/control_loops/claw/claw.q.h"
#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/constants.h"
@@ -54,7 +54,7 @@
#define M_PI 3.14159265358979323846
#endif
-using ::y2014::control_loops::drivetrain_queue;
+using ::frc971::control_loops::drivetrain_queue;
using ::y2014::control_loops::claw_queue;
using ::y2014::control_loops::shooter_queue;
@@ -470,7 +470,7 @@
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
shooter_(".y2014.control_loops.shooter_queue.output"),
- drivetrain_(".y2014.control_loops.drivetrain_queue.output") {}
+ drivetrain_(".frc971.control_loops.drivetrain_queue.output") {}
void set_pressure_switch(::std::unique_ptr<DigitalInput> pressure_switch) {
pressure_switch_ = ::std::move(pressure_switch);
@@ -566,7 +566,7 @@
::std::unique_ptr<Relay> compressor_relay_;
::aos::Queue<::y2014::control_loops::ShooterQueue::Output> shooter_;
- ::aos::Queue<::y2014::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
::std::atomic<bool> run_{true};
};
@@ -583,11 +583,11 @@
private:
virtual void Read() override {
- ::y2014::control_loops::drivetrain_queue.output.FetchAnother();
+ ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
}
virtual void Write() override {
- auto &queue = ::y2014::control_loops::drivetrain_queue.output;
+ auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
left_drivetrain_talon_->Set(-queue->left_voltage / 12.0);
right_drivetrain_talon_->Set(queue->right_voltage / 12.0);