Updated the drivetrain control loop for this year.
Still need a couple unit tests, but the current test passes.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index e9c787e..df39aca 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -5,7 +5,8 @@
'type': 'none',
'dependencies': [
'<(AOS)/build/aos_all.gyp:Atom',
- '../control_loops/control_loops.gyp:DriveTrain',
+ '../control_loops/drivetrain/drivetrain.gyp:drivetrain',
+ '../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
'../control_loops/wrist/wrist.gyp:wrist',
'../control_loops/wrist/wrist.gyp:wrist_lib_test',
'../control_loops/index/index.gyp:index',
diff --git a/frc971/control_loops/DriveTrain.mat b/frc971/control_loops/DriveTrain.mat
deleted file mode 100644
index fcc6f45..0000000
--- a/frc971/control_loops/DriveTrain.mat
+++ /dev/null
@@ -1,12 +0,0 @@
-#include "frc971/control_loops/StateFeedbackLoop.h"
-
-typedef StateFeedbackLoop<4, 2> MatrixClass;
-#define MATRIX_INIT A << 1.0000000000, 0.0095410093, 0.0000000000, -0.0000167223, 0.0000000000, 0.9096302600, 0.0000000000, -0.0032396985, 0.0000000000, -0.0000167223, 1.0000000000, 0.0095410093, 0.0000000000, -0.0032396985, 0.0000000000, 0.9096302600; \
-B << 0.0000628338, 0.0000022892, 0.0123712263, 0.0004435007, 0.0000022892, 0.0000628338, 0.0004435007, 0.0123712263; \
-C << 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000, 0.0000000000; \
-D << 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000; \
-L << 1.7496302600, -0.0032396985, 72.1296388532, -0.4369906587, -0.0032396985, 1.7496302600, -0.4369906586, 72.1296388532; \
-K << 242.8102455120, 19.7898401032, -8.7045950610, -0.9720464423, -8.7045950610, -0.9720464423, 242.8102455120, 19.7898401032; \
-U_max << 12.0000000000, 12.0000000000; \
-U_min << -12.0000000000, -12.0000000000; \
-
diff --git a/frc971/control_loops/StateFeedbackLoop.h b/frc971/control_loops/StateFeedbackLoop.h
deleted file mode 100644
index ad37f49..0000000
--- a/frc971/control_loops/StateFeedbackLoop.h
+++ /dev/null
@@ -1,131 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
-#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
-
-// wikipedia article is <http://en.wikipedia.org/wiki/State_observer>
-
-#include "Eigen/Dense"
-
-template <int number_of_states, int number_of_outputs>
-class StateFeedbackPlant {
- public:
- Eigen::Matrix<double, number_of_states, 1> X;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
- Eigen::Matrix<double, number_of_outputs, 1> U;
- Eigen::Matrix<double, number_of_outputs, 1> U_max;
- Eigen::Matrix<double, number_of_outputs, 1> U_min;
- Eigen::Matrix<double, number_of_states, number_of_states> A;
- Eigen::Matrix<double, number_of_states, number_of_outputs> B;
- Eigen::Matrix<double, number_of_outputs, number_of_states> C;
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> D;
- // TODO(aschuh): These following 2 lines are here because MATRIX_INIT
- // assumes that you have a controller as well as a plant.
- Eigen::Matrix<double, number_of_states, number_of_outputs> L;
- Eigen::Matrix<double, number_of_outputs, number_of_states> K;
-
- StateFeedbackPlant() {
- X.setZero();
- Y.setZero();
- U.setZero();
- }
-
- virtual ~StateFeedbackPlant() {}
-
- // If U is outside the hardware range, limit it before the plant tries to use
- // it.
- virtual void CapU() {
- for (int i = 0; i < number_of_outputs; ++i) {
- if (U[i] > U_max[i]) {
- U[i] = U_max[i];
- } else if (U[i] < U_min[i]) {
- U[i] = U_min[i];
- }
- }
- }
- // Computes the new X and Y given the control input.
- void Update() {
- CapU();
- X = A * X + B * U;
- Y = C * X + D * U;
- }
-
- protected:
- // these are accessible from non-templated subclasses
- static const int number_of_states_var = number_of_states;
- static const int number_of_outputs_var = number_of_outputs;
-};
-
-template <int number_of_states, int number_of_outputs>
-class StateFeedbackLoop {
- public:
- Eigen::Matrix<double, number_of_states, 1> X;
- Eigen::Matrix<double, number_of_states, 1> X_hat;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
- Eigen::Matrix<double, number_of_states, 1> R;
- Eigen::Matrix<double, number_of_outputs, 1> U;
- Eigen::Matrix<double, number_of_outputs, 1> U_max;
- Eigen::Matrix<double, number_of_outputs, 1> U_min;
- Eigen::Matrix<double, number_of_outputs, 1> U_ff;
- Eigen::Matrix<double, number_of_states, number_of_states> A;
- Eigen::Matrix<double, number_of_states, number_of_outputs> B;
- // K in wikipedia article
- Eigen::Matrix<double, number_of_outputs, number_of_states> C;
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> D;
- // B in wikipedia article
- Eigen::Matrix<double, number_of_states, number_of_outputs> L;
- // C in wikipedia article
- Eigen::Matrix<double, number_of_outputs, number_of_states> K;
-
- StateFeedbackLoop() {
- // You have to initialize all the matrices to 0 or else all their elements
- // are undefined.
- X.setZero();
- X_hat.setZero();
- Y.setZero();
- R.setZero();
- U.setZero();
- U_ff.setZero();
- }
- virtual ~StateFeedbackLoop() {}
-
- virtual void FeedForward() {
- for (int i = 0; i < number_of_outputs; ++i) {
- U_ff[i] = 0.0;
- }
- }
- // If U is outside the hardware range, limit it before it
- // gets added to the observer.
- virtual void CapU() {
- for (int i = 0; i < number_of_outputs; ++i) {
- if (U[i] > U_max[i]) {
- U[i] = U_max[i];
- } else if (U[i] < U_min[i]) {
- U[i] = U_min[i];
- }
- }
- }
- // update_observer is whether or not to use the values in Y.
- // stop_motors is whether or not to output all 0s.
- void Update(bool update_observer, bool stop_motors) {
- if (stop_motors) {
- for (int i = 0; i < number_of_outputs; ++i) {
- U[i] = 0.0;
- }
- } else {
- // new power = constant * (goal - current prediction)
- U.noalias() = K * (R - X_hat);
- CapU();
- }
-
- if (update_observer) {
- X_hat = (A - L * C) * X_hat + L * Y + B * U;
- } else {
- X_hat = A * X_hat + B * U;
- }
- }
-
- protected:
- // these are accessible from non-templated subclasses
- static const int number_of_states_var = number_of_states;
- static const int number_of_outputs_var = number_of_outputs;
-};
-#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 18df792..ea09d11 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -1,9 +1,4 @@
{
- 'variables': {
- 'loop_files': [
- 'DriveTrain.q',
- ]
- },
'targets': [
{
'target_name': 'state_feedback_loop',
@@ -19,37 +14,5 @@
'<(EXTERNALS):eigen',
],
},
- {
- 'target_name': 'control_loops',
- 'type': 'static_library',
- 'sources': ['<@(loop_files)'],
- 'variables': {
- 'header_path': 'frc971/control_loops',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'DriveTrain',
- 'type': 'executable',
- 'sources': [
- 'DriveTrain.cc',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:controls',
- 'control_loops',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/atom_code/atom_code.gyp:init',
- 'state_feedback_loop',
- ],
- },
],
}
diff --git a/frc971/control_loops/DriveTrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
similarity index 95%
rename from frc971/control_loops/DriveTrain.cc
rename to frc971/control_loops/drivetrain/drivetrain.cc
index bcae82d..af04086 100644
--- a/frc971/control_loops/DriveTrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/DriveTrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
#include <stdio.h>
#include <sched.h>
@@ -7,8 +7,9 @@
#include "aos/aos_core.h"
#include "aos/common/logging/logging.h"
#include "aos/common/queue.h"
-#include "frc971/control_loops/DriveTrain.mat"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
@@ -20,10 +21,10 @@
// Width of the robot.
const double width = 22.0 / 100.0 * 2.54;
-class DrivetrainMotorsSS : public MatrixClass {
+class DrivetrainMotorsSS : public StateFeedbackLoop<4, 2, 2> {
public:
- DrivetrainMotorsSS (void) {
- MATRIX_INIT;
+ DrivetrainMotorsSS (void)
+ : StateFeedbackLoop(MakeDrivetrainLoop()) {
_offset = 0;
_integral_offset = 0;
_left_goal = 0.0;
@@ -298,5 +299,3 @@
} // namespace control_loops
} // namespace frc971
-
-AOS_RUN_LOOP(frc971::control_loops::DrivetrainLoop)
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
new file mode 100644
index 0000000..84c4a3a
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -0,0 +1,70 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'drivetrain_loop',
+ 'type': 'static_library',
+ 'sources': ['drivetrain.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/drivetrain',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain.cc',
+ 'drivetrain_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'drivetrain_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'drivetrain_loop',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'drivetrain_loop',
+ 'drivetrain_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'drivetrain_lib',
+ 'drivetrain_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/DriveTrain.h b/frc971/control_loops/drivetrain/drivetrain.h
similarity index 94%
rename from frc971/control_loops/DriveTrain.h
rename to frc971/control_loops/drivetrain/drivetrain.h
index 61ea524..7b07247 100644
--- a/frc971/control_loops/DriveTrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,7 +2,7 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
#include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/DriveTrain.q b/frc971/control_loops/drivetrain/drivetrain.q
similarity index 100%
rename from frc971/control_loops/DriveTrain.q
rename to frc971/control_loops/drivetrain/drivetrain.q
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
new file mode 100644
index 0000000..934ba9d
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -0,0 +1,149 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the drivetrain and sends out queue messages containing the
+// position.
+class DrivetrainSimulation {
+ public:
+ // Constructs a motor simulation.
+ DrivetrainSimulation()
+ : drivetrain_plant_(
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+ my_drivetrain_loop_(".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();
+ }
+
+ // Resets the plant.
+ void Reinitialize() {
+ drivetrain_plant_->X(0, 0) = 0.0;
+ drivetrain_plant_->X(1, 0) = 0.0;
+ drivetrain_plant_->Y = drivetrain_plant_->C * drivetrain_plant_->X;
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
+ }
+
+ // Returns the position of the drivetrain.
+ double GetLeftPosition() const {
+ return drivetrain_plant_->Y(0, 0);
+ }
+ double GetRightPosition() const {
+ return drivetrain_plant_->Y(1, 0);
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double left_encoder = GetLeftPosition();
+ const double right_encoder = GetRightPosition();
+
+ ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
+ my_drivetrain_loop_.position.MakeMessage();
+ position->left_encoder = left_encoder;
+ position->right_encoder = right_encoder;
+ position.Send();
+ }
+
+ // Simulates the drivetrain moving for one timestep.
+ void Simulate() {
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
+ EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
+ drivetrain_plant_->U << my_drivetrain_loop_.output->left_voltage,
+ my_drivetrain_loop_.output->right_voltage;
+ drivetrain_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+ private:
+ Drivetrain my_drivetrain_loop_;
+ double last_left_position_;
+ double last_right_position_;
+};
+
+class DrivetrainTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // 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.
+ Drivetrain my_drivetrain_loop_;
+
+ // Create a loop and simulation plant.
+ DrivetrainLoop drivetrain_motor_;
+ DrivetrainSimulation drivetrain_motor_plant_;
+
+ DrivetrainTest() : my_drivetrain_loop_(".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"),
+ drivetrain_motor_(&my_drivetrain_loop_),
+ drivetrain_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_drivetrain_loop_.goal.FetchLatest();
+ my_drivetrain_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+ drivetrain_motor_plant_.GetLeftPosition(),
+ 1e-2);
+ EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+ drivetrain_motor_plant_.GetRightPosition(),
+ 1e-2);
+ }
+
+ virtual ~DrivetrainTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the drivetrain converges on a goal.
+TEST_F(DrivetrainTest, ConvergesCorrectly) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .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();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_main.cc b/frc971/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..a6efbcd
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::DrivetrainLoop drivetrain;
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
new file mode 100644
index 0000000..d4edba8
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.0081841122497, 0.0, -5.9473473594e-05, 0.0, 0.660289401132, 0.0, -0.0103071702002, 0.0, -5.9473473594e-05, 1.0, 0.0081841122497, 0.0, -0.0103071702002, 0.0, 0.660289401132;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000333031510271, 1.09073596255e-05, 0.0623024929693, 0.00189032194188, 1.09073596255e-05, 0.000333031510271, 0.00189032194188, 0.0623024929693;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 0, 0, 1, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlant<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.50028940113, -0.0103071702002, 41.1373728147, -1.1627040905, -0.0103071702002, 1.50028940113, -1.1627040905, 41.1373728147;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 48.196534943, -0.087263189034, -1.46233261597, -0.163410937407, -1.46233261597, -0.163410937407, 48.196534943, -0.087263189034;
+ return StateFeedbackLoop<4, 2, 2>(L, K, MakeDrivetrainPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_motor_plant.h
new file mode 100644
index 0000000..af43b46
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/matlab/drivetrain_controller.m b/frc971/control_loops/matlab/drivetrain_controller.m
index 11a5a4b..b51af99 100644
--- a/frc971/control_loops/matlab/drivetrain_controller.m
+++ b/frc971/control_loops/matlab/drivetrain_controller.m
@@ -1,12 +1,18 @@
close all;
load 'drivetrain_spin_low'
load 'drivetrain_strait_low'
+% Max power amps of CIM or maybe half the mass of the robot in lbs or the whole robot in kg.
m = 68;
+% Must be in meters. Maybe width of robot (distance of center wheels from center).
rb = 0.617998644 / 2.0;
+% Moment of Inertia
J = 7;
stall_current = 133.0;
+% Resistance of the motor, divided by the number of motors.
R = 12.0 / stall_current / 4 / 0.43;
+% Motor Constant
Km = (12.0 - R * 2.7) / (4650.0 / 60.0 * 2.0 * pi);
+% Torque Constant
Kt = 0.008;
r = 0.04445; % 3.5 inches diameter
G_low = 60.0 / 15.0 * 50.0 / 15.0;
@@ -15,6 +21,9 @@
G = G_low;
+% must refer to how each side of the robot affects the other? Units of 1 / kg
+% I think that if the center of mass is in the center of the robot, then
+% msp will evaluate to 2/(mass of robot) and msn will evaluate to 0.
msp = (1.0 / m + rb ^ 2.0 / J);
msn = (1.0 / m - rb ^ 2.0 / J);
tc = -Km * Kt * G ^ 2.0 / (R * r ^ 2.0);
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..e5a8217
--- /dev/null
+++ b/frc971/control_loops/python/drivetrain.py
@@ -0,0 +1,161 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self):
+ super(Drivetrain, self).__init__("Drivetrain")
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM. Used number from last year.
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the drivetrain in kg m^2
+ # Just borrowed from last year.
+ self.J = 7.0
+ # Mass of the robot, in kg.
+ self.m = 68
+ # Radius of the robot, in meters (from last year).
+ self.rb = 0.617998644 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = .04445
+ # Resistance of the motor, divided by the number of motors.
+ self.R = 12.0 / self.stall_current / 6
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratios
+ self.G_low = 16.0 / 60.0 * 19.0 / 50.0
+ self.G_high = 28.0 / 48.0 * 19.0 / 50.0
+ self.G = self.G_low
+ # Control loop time step
+ self.dt = 0.01
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tc = -self.Kt / self.Kv / (self.G * self.G * self.R * self.r * self.r)
+ self.mp = self.Kt / (self.G * self.R * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[position1], [velocity1], [position2], velocity2]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tc, 0, self.msn * self.tc],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tc, 0, self.msp * self.tc]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mp, self.msn * self.mp],
+ [0, 0],
+ [self.msn * self.mp, self.msp * self.mp]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ # Poles from last year.
+ self.hp = 0.8
+ self.lp = 0.85
+ self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+
+ print self.K
+
+ self.hlp = 0.07
+ self.llp = 0.09
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain()
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ pylab.plot(range(100), simulated_left)
+ pylab.plot(range(100), simulated_right)
+ pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ 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])
+
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ 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])
+
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ 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])
+
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ drivetrain.DumpHeaderFile(argv[1])
+ drivetrain.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index e0cfbac..6951760 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -8,7 +8,7 @@
#include "aos/atom_code/input/JoystickInput.h"
#include "frc971/input/AutoMode.q.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
index fd14df4..e66dea0 100644
--- a/frc971/input/SensorReader.cc
+++ b/frc971/input/SensorReader.cc
@@ -6,7 +6,7 @@
#include "aos/common/inttypes.h"
#include "aos/common/input/SensorInput.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/sensor_values.h"
#define M_PI 3.14159265358979323846
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index c62365b..a7086fb 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -24,7 +24,7 @@
'<(AOS)/atom_code/input/input.gyp:joystick',
'<(AOS)/common/network/network.gyp:socket',
'actions',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/atom_code/atom_code.gyp:init',
],
@@ -37,7 +37,7 @@
],
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/network/network.gyp:socket',
],
@@ -57,7 +57,6 @@
],
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
],
},
@@ -81,7 +80,6 @@
],
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'actions',
# TODO(brians) this shouldn't need to be here
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/AtomMotorWriter.cc
index 243ac81..add20d4 100644
--- a/frc971/output/AtomMotorWriter.cc
+++ b/frc971/output/AtomMotorWriter.cc
@@ -9,7 +9,7 @@
#include "aos/atom_code/output/MotorOutput.h"
#include "frc971/queues/Piston.q.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/constants.h"
using ::frc971::control_loops::drivetrain;
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 780eadf..fc2def4 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -41,7 +41,7 @@
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/network/network.gyp:socket',
],