Add elevator control loop and test.
This commit includes a big chunk of elevator code that Comran worked on,
and all the zeroing code that Adam got working for the bot3.
Unfortunately, we did not split these two projects apart earlier, so
this is a big commit containing all of the code combined. Right now, all
of the elevator tests that aren't commented out pass with the hall
effect zeroing method that Adam made, but all the tests in
elevator_lib_test will need to be checked by other programmers on the
team & updated as needed in a future commit.
Change-Id: I4ad6247303c3a939a28eff7d2043de74a2d3865d
diff --git a/bot3/control_loops/control_loops.gyp b/bot3/control_loops/control_loops.gyp
new file mode 100644
index 0000000..b63816e
--- /dev/null
+++ b/bot3/control_loops/control_loops.gyp
@@ -0,0 +1,15 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'position_sensor_sim',
+ 'type': 'static_library',
+ 'sources': ['position_sensor_sim.cc'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops',
+ },
+ 'dependencies': [
+ '<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_queue',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/elevator/elevator.cc b/bot3/control_loops/elevator/elevator.cc
new file mode 100644
index 0000000..49433a7
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.cc
@@ -0,0 +1,280 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include <cmath>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/elevator/integral_elevator_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+void SimpleCappedStateFeedbackLoop::CapU() {
+ mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
+ mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
+}
+
+double SimpleCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
+ // Compute K matrix to compensate for position errors.
+ double Kp = K(0, 0);
+
+ // Compute how much we need to change R in order to achieve the change in U
+ // that was observed.
+ return -(1.0 / Kp) * (U_uncapped() - U())(0, 0);
+}
+
+Elevator::Elevator(control_loops::ElevatorQueue *elevator)
+ : aos::controls::ControlLoop<control_loops::ElevatorQueue>(elevator),
+ loop_(new SimpleCappedStateFeedbackLoop(MakeIntegralElevatorLoop())),
+ profile_(::aos::controls::kLoopFrequency) {}
+
+bool Elevator::CheckZeroed() {
+ return state_ == RUNNING;
+}
+
+void Elevator::Correct() {
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << current_position();
+ loop_->Correct(Y);
+}
+
+double Elevator::current_position() {
+ return current_position_.encoder + offset_;
+}
+
+double Elevator::GetZeroingVelocity() {
+ return zeroing_velocity_;
+}
+
+// If the hall_effect is true that means we need to move up until is false.
+// Then we should move down.
+double Elevator::FindZeroingVelocity() {
+ if (glitch_filter_.filtered_value()) {
+ zeroing_velocity_ = kZeroingSlowVelocity;
+ } else {
+ zeroing_velocity_ = -kZeroingVelocity;
+ }
+
+ return zeroing_velocity_;
+}
+
+double Elevator::UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+
+void Elevator::SetOffset(double offset) {
+ LOG(INFO, "Changing Elevator offset from %f to %f\n", offset_, offset);
+ double doffset = offset - offset_;
+
+ loop_->mutable_X_hat(0, 0) += doffset;
+
+ // Modify the zeroing goal.
+ goal_ += doffset;
+
+ // Update the cached offset values to the actual values.
+ offset_ = offset;
+}
+
+void Elevator::RunIteration(
+ const control_loops::ElevatorQueue::Goal *unsafe_goal,
+ const control_loops::ElevatorQueue::Position *position,
+ control_loops::ElevatorQueue::Output *output,
+ control_loops::ElevatorQueue::Status *status) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ state_ = UNINITIALIZED;
+ }
+ glitch_filter_.Update(position->bottom_hall_effect, position->encoder);
+
+ // Bool to track if we should turn the motors on or not.
+ bool disable = output == nullptr;
+
+ // Save the current position so it can be used easily in the class.
+ current_position_ = *position;
+
+ if (state_ != UNINITIALIZED) {
+ Correct();
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ LOG(DEBUG, "Uninitialized\n");
+ // Startup. Assume that we are at the origin everywhere.
+ offset_ = -position->encoder;
+ loop_->mutable_X_hat().setZero();
+ LOG(INFO, "Initializing elevator offset to %f\n", offset_);
+ Correct();
+ state_ = ZEROING;
+ disable = true;
+ glitch_filter_.Reset(position->bottom_hall_effect);
+ break;
+
+ case ZEROING:
+ LOG(DEBUG, "Zeroing elevator\n");
+
+ if (glitch_filter_.negedge()) {
+ state_ = RUNNING;
+ SetOffset(-glitch_filter_.negedge_value() + kHallEffectPosition);
+ }
+
+ // We need to check FindZeroingVelocity() every time
+ // in order for zeroing to work.
+ {
+ double zeroing_velocity_temp = FindZeroingVelocity();
+ if (state_ != RUNNING && !disable) {
+ // Move the elevator either up or down based on where the zeroing hall
+ // effect is located.
+
+ goal_velocity_ = zeroing_velocity_temp;
+ goal_ += goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ }
+ }
+
+ // Bypass motion profiles while we are zeroing.
+ // This is also an important step right after the elevator is zeroed and
+ // we reach into the elevator's state matrix and change it based on the
+ // newly-obtained offset.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << goal_, goal_velocity_;
+ profile_.MoveCurrentState(current);
+ }
+ break;
+
+ case RUNNING:
+ if (unsafe_goal) {
+ profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_velocity, 0.50));
+ profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_acceleration, 2.0));
+
+ // Use the profiles to limit the elevator's movements.
+ const double unfiltered_goal = ::std::max(
+ ::std::min(unsafe_goal->height, kElevUpperLimit), kElevLowerLimit);
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unfiltered_goal, unsafe_goal->velocity);
+ goal_ = goal_state(0, 0);
+ goal_velocity_ = goal_state(1, 0);
+ }
+
+ if (state_ != RUNNING && state_ != ESTOP) {
+ state_ = UNINITIALIZED;
+ }
+ break;
+
+ case ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Limit the goals so we can't exceed the hardware limits if we are RUNNING.
+ if (state_ == RUNNING) {
+ // Limit the elevator goal to min/max allowable heights.
+ if (goal_ >= kElevUpperLimit) {
+ LOG(WARNING, "Elevator goal above limit, %f > %f\n", goal_,
+ kElevUpperLimit);
+ goal_ = kElevUpperLimit;
+ }
+
+ if (goal_ <= kElevLowerLimit) {
+ LOG(WARNING, "Elevator goal below limit, %f < %f\n", goal_,
+ kElevLowerLimit);
+ goal_ = kElevLowerLimit;
+ }
+ }
+
+ // Check the hard limits.
+ if (state_ == RUNNING) {
+ if (current_position() >= kElevUpperHardLimit) {
+ LOG(ERROR, "Elevator at %f out of bounds [%f, %f], ESTOPing\n",
+ current_position(), kElevLowerHardLimit, kElevUpperHardLimit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+
+ if (current_position() <= kElevLowerHardLimit) {
+ LOG(ERROR, "Elevator at %f out of bounds [%f, %f], ESTOPing\n",
+ current_position(), kElevLowerHardLimit, kElevUpperHardLimit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+ }
+
+ // Set the goals.
+ loop_->mutable_R() << goal_, goal_velocity_, 0.0;
+
+ const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
+ loop_->set_max_voltage(max_voltage);
+
+ if (state_ == ESTOP) {
+ disable = true;
+ }
+ loop_->Update(disable);
+
+ if (state_ == ZEROING || state_ == RUNNING) {
+ if (loop_->U() != loop_->U_uncapped()) {
+ double deltaR = loop_->UnsaturateOutputGoalChange();
+
+ // Move the elevator goal by the amount observed.
+ LOG(WARNING, "Moving elevator goal by %f to handle saturation\n", deltaR);
+ goal_ += deltaR;
+
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << goal_, goal_velocity_;
+ profile_.MoveCurrentState(current);
+ }
+ }
+
+ if (output) {
+ output->elevator = loop_->U(0, 0);
+ }
+
+ status->zeroed = state_ == RUNNING;
+
+ status->height = loop_->X_hat(0, 0);
+ status->velocity = loop_->X_hat(1, 0);
+
+ status->goal_height = goal_;
+ status->goal_velocity = goal_velocity_;
+
+ status->estopped = (state_ == ESTOP);
+ status->state = state_;
+}
+
+void GlitchFilter::Update(bool hall_effect, double encoder) {
+ posedge_ = false;
+ negedge_ = false;
+ if (hall_effect != accepted_value_) {
+ if (count_ == 0) {
+ first_encoder_ = encoder;
+ }
+ ++count_;
+ } else {
+ last_encoder_ = encoder;
+ count_ = 0;
+ }
+ if (count_ >= 2) {
+ if (hall_effect) {
+ posedge_ = true;
+ posedge_value_ = (first_encoder_ + last_encoder_) / 2.0;
+ } else {
+ negedge_ = true;
+ negedge_value_ = (first_encoder_ + last_encoder_) / 2.0;
+ }
+ accepted_value_ = hall_effect;
+ count_ = 0;
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator.gyp b/bot3/control_loops/elevator/elevator.gyp
new file mode 100644
index 0000000..c184429
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.gyp
@@ -0,0 +1,71 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'elevator_queue',
+ 'type': 'static_library',
+ 'sources': ['elevator.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/elevator',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'elevator_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'elevator.cc',
+ 'elevator_motor_plant.cc',
+ 'integral_elevator_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'elevator_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/voltage_cap/voltage_cap.gyp:voltage_cap',
+ ],
+ 'export_dependent_settings': [
+ 'elevator_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'elevator_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'elevator_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'elevator_lib',
+ '<(DEPTH)/bot3/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ {
+ 'target_name': 'elevator',
+ 'type': 'executable',
+ 'sources': [
+ 'elevator_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'elevator_lib',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/elevator/elevator.h b/bot3/control_loops/elevator/elevator.h
new file mode 100644
index 0000000..3acd1c8
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.h
@@ -0,0 +1,193 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+class ElevatorTest_DisabledGoalTest_Test;
+class ElevatorTest_ElevatorGoalPositiveWindupTest_Test;
+class ElevatorTest_ElevatorGoalNegativeWindupTest_Test;
+class ElevatorTest_PositiveRunawayProfileTest_Test;
+class ElevatorTest_NegativeRunawayProfileTest_Test;
+} // namespace testing
+
+// Constants
+constexpr double kZeroingVoltage = 4.0;
+// TODO(austin): Slow down the zeroing velocity.
+constexpr double kZeroingVelocity = 0.05;
+constexpr double kZeroingSlowVelocity = 0.01;
+
+// Game pieces
+constexpr double kToteHeight = 0.3;
+
+// TODO(comran): Get actual constants for the ones below.
+// Gearing
+
+constexpr double kElevEncoderRatio = 14.0 / 84.0;
+const int kElevGearboxOutputPulleyTeeth = 32; // 32 teeth
+constexpr double kElevGearboxOutputPitch = 0.005; // 5 mm/tooth
+
+constexpr double kElevGearboxOutputRadianDistance =
+ kElevGearboxOutputPulleyTeeth * kElevGearboxOutputPitch / (2.0 * M_PI);
+
+// Limits
+constexpr double kElevLowerHardLimit = -0.005;
+constexpr double kElevUpperHardLimit = 0.720000;
+constexpr double kElevLowerLimit = 0.010000;
+constexpr double kElevUpperLimit = 0.680000;
+
+// Zeroing
+namespace {
+// TODO(Adam): Find the actual value of the hall effect position.
+constexpr double kHallEffectPosition = 0.01;
+} // namespace
+// End constants
+
+class SimpleCappedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+ SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> &&loop)
+ : StateFeedbackLoop<3, 1, 1>(::std::move(loop)), max_voltage_(12.0) {}
+
+ void set_max_voltage(double max_voltage) {
+ max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
+ }
+
+ void CapU() override;
+
+ // Returns the amount to change the position goal in order to no longer
+ // saturate the controller.
+ double UnsaturateOutputGoalChange();
+
+ private:
+ double max_voltage_;
+};
+
+// The GlitchFilter filters out single cycle changes in order to filter out
+// sensor noise. It also captures the sensor value at the time that the
+// original transition happens (not the de-bounced transition) to remove delay
+// caused by this filter.
+class GlitchFilter {
+ public:
+ void Reset(bool hall_effect) {
+ accepted_value_ = hall_effect;
+ posedge_ = false;
+ negedge_ = false;
+ count_ = 0;
+ }
+
+ // Updates the filter state with new observations.
+ void Update(bool hall_effect, double encoder);
+
+ // Returns a debounced hall effect value.
+ bool filtered_value() const { return accepted_value_; }
+
+ // Returns true if last cycle had a posedge.
+ bool posedge() const { return posedge_; }
+ // Returns the encoder value across the last posedge.
+ double posedge_value() const { return posedge_value_; }
+
+ // Returns true if last cycle had a negedge.
+ bool negedge() const { return negedge_; }
+ // Returns the encoder value across the last negedge.
+ double negedge_value() const { return negedge_value_; }
+
+ private:
+ int count_ = 0;
+ bool accepted_value_ = false;
+ bool posedge_ = false;
+ bool negedge_ = false;
+ double posedge_value_ = 0;
+ double negedge_value_ = 0;
+
+ double first_encoder_ = 0;
+ double last_encoder_ = 0;
+};
+
+class Elevator
+ : public aos::controls::ControlLoop<control_loops::ElevatorQueue> {
+ public:
+ explicit Elevator(control_loops::ElevatorQueue *elevator_queue =
+ &control_loops::elevator_queue);
+
+ bool CheckZeroed();
+
+ // Getter for the current elevator positions with zeroed offset.
+ double current_position();
+
+ double GetZeroingVelocity();
+
+ enum State {
+ // Waiting to receive data before doing anything.
+ UNINITIALIZED = 0,
+ // Moving the elevator to find an index pulse.
+ ZEROING = 2,
+ // All good!
+ RUNNING = 3,
+ // Internal error caused the elevator to abort.
+ ESTOP = 4,
+ };
+
+ State state() const { return state_; }
+
+ protected:
+ void RunIteration(const control_loops::ElevatorQueue::Goal *goal,
+ const control_loops::ElevatorQueue::Position *position,
+ control_loops::ElevatorQueue::Output *output,
+ control_loops::ElevatorQueue::Status *status) override;
+
+ private:
+ friend class testing::ElevatorTest_DisabledGoalTest_Test;
+ friend class testing::ElevatorTest_ElevatorGoalPositiveWindupTest_Test;
+ friend class testing::ElevatorTest_ElevatorGoalNegativeWindupTest_Test;
+ friend class testing::ElevatorTest_PositiveRunawayProfileTest_Test;
+ friend class testing::ElevatorTest_NegativeRunawayProfileTest_Test;
+
+ void SetOffset(double offset);
+
+ // Returns the current elevator zeroing velocity.
+ double FindZeroingVelocity();
+
+ // Corrects the Observer with the current position.
+ void Correct();
+
+ double UseUnlessZero(double target_value, double default_value);
+
+ // The state feedback control loop or loops to talk to.
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop> loop_;
+
+ // Offsets from the encoder position to the absolute position. Add these to
+ // the encoder position to get the absolute position.
+ double offset_ = 0.0;
+
+ bool zeroed_ = false;
+ State state_ = State::UNINITIALIZED;
+
+ // Variables for detecting the zeroing hall effect edge.
+ bool zeroing_hall_effect_edge_detector_initialized_ = false;
+ bool zeroing_hall_effect_previous_reading_ = false;
+
+ // Current velocity to move at while zeroing.
+ double zeroing_velocity_ = 0.0;
+
+ // The goals for the elevator.
+ double goal_ = 0.0;
+ double goal_velocity_ = 0.0;
+
+ control_loops::ElevatorQueue::Position current_position_;
+
+ aos::util::TrapezoidProfile profile_;
+ GlitchFilter glitch_filter_;
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ELEVATOR_H_
diff --git a/bot3/control_loops/elevator/elevator.q b/bot3/control_loops/elevator/elevator.q
new file mode 100644
index 0000000..663bb59
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.q
@@ -0,0 +1,63 @@
+package bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ElevatorQueue {
+ implements aos.control_loops.ControlLoop;
+
+ // Elevator heights are the vertical distance (in meters) from a defined zero.
+
+ message Goal {
+ // Height of the elevator.
+ double height;
+
+ // Linear velocity of the elevator.
+ float velocity;
+
+ // Maximum elevator profile velocity or 0 for the default.
+ float max_velocity;
+
+ // Maximum elevator profile acceleration or 0 for the default.
+ float max_acceleration;
+ };
+
+ message Position {
+ // bottom hall effect sensor.
+ bool bottom_hall_effect;
+ // encoders used for zeroing.
+ double encoder;
+ };
+
+ message Status {
+ // Is the elevator zeroed?
+ bool zeroed;
+
+ // Estimated height of the elevator.
+ double height;
+
+ // Estimated velocity of the elevator.
+ float velocity;
+
+ // Goal height and velocity of the elevator.
+ double goal_height;
+ float goal_velocity;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // The internal state of the state machine.
+ int32_t state;
+ };
+
+ message Output {
+ double elevator;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ElevatorQueue elevator_queue;
diff --git a/bot3/control_loops/elevator/elevator_lib_test.cc b/bot3/control_loops/elevator/elevator_lib_test.cc
new file mode 100644
index 0000000..d174567
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_lib_test.cc
@@ -0,0 +1,545 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include <math.h>
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/time.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "bot3/control_loops/position_sensor_sim.h"
+#include "bot3/control_loops/elevator/elevator_motor_plant.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+using ::aos::time::Time;
+using ::bot3::control_loops::PositionSensorSimulator;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// TODO(comran+adam): Check these over with Austin, Ben, Brian, and others to
+// make sure we didn't forget anything -- especially the zeroing tests!!!
+
+class ElevatorSimulator {
+ public:
+ ElevatorSimulator()
+ : plant_(new StateFeedbackPlant<2, 1, 1>(MakeElevatorPlant())),
+ position_sim_(),
+ queue_(".bot3.control_loops.elevator_queue", 0xca8daa3b,
+ ".bot3.control_loops.elevator_queue.goal",
+ ".bot3.control_loops.elevator_queue.position",
+ ".bot3.control_loops.elevator_queue.output",
+ ".bot3.control_loops.elevator_queue.status") {
+ // Initialize the elevator.
+ InitializePosition(kElevLowerLimit);
+ }
+
+ void InitializePosition(double start_pos) {
+ InitializePosition(start_pos, kHallEffectPosition);
+ }
+
+ void InitializePosition(double start_pos, double hall_effect_position) {
+ plant_->mutable_X(0, 0) = start_pos;
+ plant_->mutable_X(1, 0) = 0.0;
+ position_sim_.Initialize(start_pos, hall_effect_position);
+ }
+
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ElevatorQueue::Position> position =
+ queue_.position.MakeMessage();
+ position_sim_.GetSensorValues(position.get());
+ position.Send();
+ }
+
+ void set_acceleration_offset(double acceleration_offset) {
+ acceleration_offset_ = acceleration_offset;
+ }
+
+ double height() const { return plant_->Y(0, 0); }
+
+ void Simulate() {
+ EXPECT_TRUE(queue_.output.FetchLatest());
+
+ plant_->mutable_U() << queue_.output->elevator;
+
+ plant_->Update();
+ plant_->mutable_X()(1, 0) += acceleration_offset_ * 0.005;
+
+ const double height = plant_->Y(0, 0);
+
+ position_sim_.MoveTo(height);
+
+ EXPECT_GE(height, kElevLowerHardLimit);
+ EXPECT_LE(height, kElevUpperHardLimit);
+ }
+
+ void MoveTo(double position) { position_sim_.MoveTo(position); }
+
+ private:
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> plant_;
+
+ PositionSensorSimulator position_sim_;
+
+ ElevatorQueue queue_;
+ double acceleration_offset_ = 0.0;
+};
+
+class ElevatorTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ ElevatorTest()
+ : queue_(".bot3.control_loops.elevator_queue", 0xca8daa3b,
+ ".bot3.control_loops.elevator_queue.goal",
+ ".bot3.control_loops.elevator_queue.position",
+ ".bot3.control_loops.elevator_queue.output",
+ ".bot3.control_loops.elevator_queue.status"),
+ elevator_(&queue_),
+ plant_() {
+ }
+
+ // Checks to see if fetching position/status from queues does not return null
+ // pointers.
+ void VerifyNearGoal() {
+ queue_.goal.FetchLatest();
+ queue_.status.FetchLatest();
+ EXPECT_TRUE(queue_.goal.get() != nullptr);
+ EXPECT_TRUE(queue_.status.get() != nullptr);
+ EXPECT_NEAR(queue_.goal->height, queue_.status->height, 0.001);
+ EXPECT_NEAR(queue_.goal->height, plant_.height(), 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ plant_.SendPositionMessage();
+ elevator_.Iterate();
+ plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to shared memory that
+ // is no longer valid.
+ ElevatorQueue queue_;
+
+ // Create a control loop and simulation.
+ Elevator elevator_;
+ ElevatorSimulator plant_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(ElevatorTest, DoesNothing) {
+ // Set the goals to the starting values. This should theoretically guarantee
+ // that the controller does nothing.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(kElevLowerLimit)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ // Run a few iterations.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
+TEST_F(ElevatorTest, ReachesGoal) {
+ // Set a reasonable goal.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(kElevLowerLimit + 0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ // Give it a lot of time to get there.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop doesn't try and go beyond the physical range of the
+// mechanisms.
+TEST_F(ElevatorTest, RespectsRange) {
+ // We're going to send the elevator to -5, which should be significantly too
+ // low.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(-5.0)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ queue_.status.FetchLatest();
+ EXPECT_NEAR(kElevLowerLimit, queue_.status->height, 0.001);
+
+ // We're going to give the elevator some ridiculously high goal.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(50.0)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ queue_.status.FetchLatest();
+ EXPECT_NEAR(kElevUpperLimit, queue_.status->height, 0.001);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(ElevatorTest, ZeroTest) {
+ queue_.goal.MakeWithBuilder()
+ .height(0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send();
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the lower hardstops doesn't cause an abort.
+TEST_F(ElevatorTest, LowerHardstopStartup) {
+ plant_.InitializePosition(kElevLowerHardLimit);
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InMS(5000));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(ElevatorTest, UpperHardstopStartup) {
+ plant_.InitializePosition(kElevUpperHardLimit);
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InMS(20000));
+
+ VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(ElevatorTest, ResetTest) {
+ plant_.InitializePosition(kElevLowerLimit);
+ queue_.goal.MakeWithBuilder().height(0.05).Send();
+ RunForTime(Time::InMS(5000));
+
+ EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+ VerifyNearGoal();
+ SimulateSensorReset();
+ RunForTime(Time::InMS(100));
+ EXPECT_NE(Elevator::RUNNING, elevator_.state());
+ RunForTime(Time::InMS(10000));
+ EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+ VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(ElevatorTest, DisabledGoalTest) {
+ queue_.goal.MakeWithBuilder().height(0.45).Send();
+
+ RunForTime(Time::InMS(100), false);
+ EXPECT_EQ(0.0, elevator_.goal_);
+
+ // Now make sure they move correctly
+ RunForTime(Time::InMS(4000), true);
+ EXPECT_NE(0.0, elevator_.goal_);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(ElevatorTest, ElevatorGoalPositiveWindupTest) {
+ plant_.InitializePosition(0.0, kHallEffectPosition);
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::ZEROING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_goal = elevator_.goal_;
+ elevator_.goal_ += 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::ZEROING);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(ElevatorTest, ElevatorGoalNegativeWindupTest) {
+ plant_.InitializePosition(0.0, kHallEffectPosition);
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::ZEROING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_goal = elevator_.goal_;
+ elevator_.goal_ -= 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::ZEROING);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(ElevatorTest, ZeroNoGoal) {
+ RunForTime(Time::InSeconds(5));
+
+ EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+}
+
+// Zeroing tests
+// TODO(comran+adam): The following tests don't look like they are complete, so
+// we will need to get these tests checked & passed before running the elevator
+// control loop.
+
+// Tests that starting in the middle zeros correctly.
+TEST_F(ElevatorTest, CasualStart) {
+ const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.5).Send();
+
+ // Find a reasonable value for the time.
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that a start position above the upper soft limit zeros correctly.
+TEST_F(ElevatorTest, AboveSoftLimitStart) {
+ const double start_position = (kElevUpperHardLimit + kElevUpperLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.5).Send();
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that a start position below the lower soft limit zeros correctly.
+TEST_F(ElevatorTest, BelowSoftLimitStart) {
+ const double start_position = (kElevLowerHardLimit + kElevLowerLimit) / 2;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests it zeroes when we start above the hall effect sensor.
+TEST_F(ElevatorTest, OverHallEffect) {
+ const double start_position = kHallEffectPosition;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+ queue_.goal.MakeWithBuilder().height(0.4).Send();
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that we go down fast, up slow, and then get zeroed.
+TEST_F(ElevatorTest, DownAndUp) {
+ const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ EXPECT_TRUE(elevator_.CheckZeroed());
+
+ // Make sure we ended off the hall effect.
+ queue_.position.FetchLatest();
+ EXPECT_TRUE(queue_.position.get() != nullptr);
+ EXPECT_FALSE(queue_.position->bottom_hall_effect);
+}
+
+// Verify that we can zero while disabled.
+TEST_F(ElevatorTest, ZeroWhileDisabled) {
+ const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+ plant_.InitializePosition(start_position, kHallEffectPosition);
+
+ // Running iteration while disabled
+ RunIteration(false);
+ RunIteration(false);
+
+ // Move elevator below hall effect.
+ plant_.MoveTo(kHallEffectPosition - 0.1);
+ RunIteration(false);
+ plant_.MoveTo(kHallEffectPosition - 0.1);
+ RunIteration(false);
+ // Make sure it only zeroes while going up.
+ EXPECT_TRUE(!elevator_.CheckZeroed());
+ // Move above the hall effect.
+ plant_.MoveTo(kHallEffectPosition + 0.1);
+ RunIteration(false);
+ RunIteration(false);
+ // Make sure we are zeroed.
+ EXPECT_TRUE(elevator_.CheckZeroed());
+}
+
+// Tests that the loop can reach a goal with a constant force.
+TEST_F(ElevatorTest, ReachesGoalWithIntegral) {
+ // Set a reasonable goal.
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .height(kElevLowerLimit + 0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send());
+
+ plant_.set_acceleration_offset(0.1);
+ // Give it a lot of time to get there.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the goal (with motion profile) doesn't run away from the elevator
+TEST_F(ElevatorTest, PositiveRunawayProfileTest) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+
+
+ double orig_goal = elevator_.goal_;
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << elevator_.goal_ + 100.0, elevator_.goal_velocity_;
+ elevator_.profile_.MoveCurrentState(current);
+ }
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::RUNNING);
+}
+
+// Tests that the goal (with motion profile) doesn't run away from the elevator
+TEST_F(ElevatorTest, NegativeRunawayProfileTest) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+ while (elevator_.state() != Elevator::RUNNING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+
+
+ double orig_goal = elevator_.goal_;
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << elevator_.goal_ - 100.0, elevator_.goal_velocity_;
+ elevator_.profile_.MoveCurrentState(current);
+ }
+ RunIteration();
+ EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+ RunIteration();
+ EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+ EXPECT_EQ(elevator_.state(), Elevator::RUNNING);
+}
+
+// Tests that the glitch filter handles positive edges.
+TEST(GlitchFilterTest, PosedgeDetect) {
+ GlitchFilter filter;
+ filter.Reset(false);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+
+ filter.Update(false, 0.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+
+ filter.Update(true, 1.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+
+ filter.Update(true, 2.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_TRUE(filter.posedge());
+
+ filter.Update(true, 3.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.posedge());
+ EXPECT_EQ(0.5, filter.posedge_value());
+}
+
+// Tests that the glitch filter handles negative edges.
+TEST(GlitchFilterTest, NegedgeDetect) {
+ GlitchFilter filter;
+ filter.Reset(true);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+
+ filter.Update(true, 0.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+
+ filter.Update(false, 1.0);
+ EXPECT_TRUE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+
+ filter.Update(false, 2.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_TRUE(filter.negedge());
+
+ filter.Update(false, 3.0);
+ EXPECT_FALSE(filter.filtered_value());
+ EXPECT_FALSE(filter.negedge());
+ EXPECT_EQ(0.5, filter.negedge_value());
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator_main.cc b/bot3/control_loops/elevator/elevator_main.cc
new file mode 100644
index 0000000..f7a5eef
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::bot3::control_loops::Elevator elevator;
+ elevator.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.cc b/bot3/control_loops/elevator/elevator_motor_plant.cc
new file mode 100644
index 0000000..d3985b6
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "bot3/control_loops/elevator/elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00329835431624, 0.0, 0.407009515002;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000238685884904, 0.0831773970353;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeElevatorController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 0.843942422954, 1.30830010079;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 327.58799433, 4.77067036522;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.0, -0.00810387520358, 0.0, 2.4569450176;
+ return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeElevatorPlantCoefficients()));
+ return StateFeedbackPlant<2, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeElevatorController()));
+ return StateFeedbackLoop<2, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.h b/bot3/control_loops/elevator/elevator_motor_plant.h
new file mode 100644
index 0000000..d87c79e
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeElevatorController();
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.cc b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
new file mode 100644
index 0000000..fba91c1
--- /dev/null
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "bot3/control_loops/elevator/integral_elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00329835431624, 0.000238685884904, 0.0, 0.407009515002, 0.0831773970353, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.000238685884904, 0.0831773970353, 0.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeIntegralElevatorController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 0.819604251485, 7.84671425417, 56.221891957;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 327.58799433, 4.77067036522, 1.0;
+ Eigen::Matrix<double, 3, 3> A_inv;
+ A_inv << 1.0, -0.00810387520358, 0.000435373360429, 0.0, 2.4569450176, -0.204362291223, 0.0, 0.0, 1.0;
+ return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeIntegralElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeIntegralElevatorPlantCoefficients()));
+ return StateFeedbackPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<3, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeIntegralElevatorController()));
+ return StateFeedbackLoop<3, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.h b/bot3/control_loops/elevator/integral_elevator_motor_plant.h
new file mode 100644
index 0000000..64a4539
--- /dev/null
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeIntegralElevatorController();
+
+StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/position_sensor_sim.cc b/bot3/control_loops/position_sensor_sim.cc
new file mode 100644
index 0000000..480cd98
--- /dev/null
+++ b/bot3/control_loops/position_sensor_sim.cc
@@ -0,0 +1,28 @@
+#include "bot3/control_loops/position_sensor_sim.h"
+
+#include <cmath>
+
+namespace bot3 {
+namespace control_loops {
+
+void PositionSensorSimulator::Initialize(double start_position,
+ double hall_effect_position) {
+ hall_effect_position_ = hall_effect_position;
+ start_position_ = start_position;
+ cur_pos_ = start_position;
+}
+
+void PositionSensorSimulator::MoveTo(double new_pos) { cur_pos_ = new_pos; }
+
+void PositionSensorSimulator::GetSensorValues(
+ control_loops::ElevatorQueue::Position* position) {
+ position->encoder = cur_pos_ - start_position_;
+ if (cur_pos_ <= hall_effect_position_) {
+ position->bottom_hall_effect = true;
+ } else {
+ position->bottom_hall_effect = false;
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/position_sensor_sim.h b/bot3/control_loops/position_sensor_sim.h
new file mode 100644
index 0000000..f025010
--- /dev/null
+++ b/bot3/control_loops/position_sensor_sim.h
@@ -0,0 +1,47 @@
+#ifndef BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#define BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+// NOTE: All encoder values in this class are assumed to be in
+// translated SI units.
+
+class PositionSensorSimulator {
+ public:
+
+ // Set new parameters for the sensors. This is useful for unit tests to change
+ // the simulated sensors' behavior on the fly.
+ // start_position: The position relative to absolute zero where the simulated
+ // structure starts. For example, to simulate the elevator
+ // starting at 40cm above absolute zero, set this to 0.4.
+ void Initialize(double start_position, double hall_effect_position);
+
+ // Simulate the structure moving to a new position. The new value is measured
+ // relative to absolute zero. This will update the simulated sensors with new
+ // readings.
+ // new_position: The new position relative to absolute zero.
+ void MoveTo(double new_position);
+
+ // Get the current values of the simulated sensors.
+ // values: The target structure will be populated with simulated sensor
+ // readings. The readings will be in SI units. For example the units
+ // can be given in radians, meters, etc.
+ void GetSensorValues(control_loops::ElevatorQueue::Position* position);
+
+ private:
+ // the position of the bottom hall effect sensor.
+ double hall_effect_position_;
+ // Current position of the mechanism relative to absolute zero.
+ double cur_pos_;
+ // Starting position of the mechanism relative to absolute zero. See the
+ // `starting_position` parameter in the constructor for more info.
+ double start_position_;
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif /* BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_ */
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
deleted file mode 100644
index 881880f..0000000
--- a/bot3/control_loops/python/control_loop.py
+++ /dev/null
@@ -1,338 +0,0 @@
-import controls
-import numpy
-
-class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double";
- self.formatToType['%d'] = "int";
- def __str__ (self):
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (self.formatToType[self.formatt], self.name, self.value)
-
-
-class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
- """Constructs a control loop writer.
-
- Args:
- gain_schedule_name: string, Name of the overall controller.
- loops: array[ControlLoop], a list of control loops to gain schedule
- in order.
- namespaces: array[string], a list of names of namespaces to nest in
- order. If None, the default will be used.
- """
- self._gain_schedule_name = gain_schedule_name
- self._loops = loops
- if namespaces:
- self._namespaces = namespaces
- else:
- self._namespaces = ['frc971', 'control_loops']
-
- self._namespace_start = '\n'.join(
- ['namespace %s {' % name for name in self._namespaces])
-
- self._namespace_end = '\n'.join(
- ['} // namespace %s' % name for name in reversed(self._namespaces)])
-
- self._constant_list = []
-
- def AddConstant(self, constant):
- """Adds a constant to write.
-
- Args:
- constant: Constant, the constant to add to the header.
- """
- self._constant_list.append(constant)
-
- def _TopDirectory(self):
- return self._namespaces[0]
-
- def _HeaderGuard(self, header_file):
- return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
- header_file.upper().replace('.', '_').replace('/', '_') +
- '_')
-
- def Write(self, header_file, cc_file):
- """Writes the loops to the specified files."""
- self.WriteHeader(header_file)
- self.WriteCC(header_file, cc_file)
-
- def _GenericType(self, typename):
- """Returns a loop template using typename for the type."""
- num_states = self._loops[0].A.shape[0]
- num_inputs = self._loops[0].B.shape[1]
- num_outputs = self._loops[0].C.shape[0]
- return '%s<%d, %d, %d>' % (
- typename, num_states, num_inputs, num_outputs)
-
- def _ControllerType(self):
- """Returns a template name for StateFeedbackController."""
- return self._GenericType('StateFeedbackController')
-
- def _LoopType(self):
- """Returns a template name for StateFeedbackLoop."""
- return self._GenericType('StateFeedbackLoop')
-
- def _PlantType(self):
- """Returns a template name for StateFeedbackPlant."""
- return self._GenericType('StateFeedbackPlant')
-
- def _CoeffType(self):
- """Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType('StateFeedbackPlantCoefficients')
-
- def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
- """Writes the header file to the file named header_file.
- Set double_appendage to true in order to include a ratio of
- moments of inertia constant. Currently, only used for 2014 claw."""
- with open(header_file, 'w') as fd:
- header_guard = self._HeaderGuard(header_file)
- fd.write('#ifndef %s\n'
- '#define %s\n\n' % (header_guard, header_guard))
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- fd.write('\n')
-
- fd.write(self._namespace_start)
-
- for const in self._constant_list:
- fd.write(str(const))
-
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlantHeader())
- fd.write('\n')
- fd.write(loop.DumpControllerHeader())
- fd.write('\n')
-
- fd.write('%s Make%sPlant();\n\n' %
- (self._PlantType(), self._gain_schedule_name))
-
- fd.write('%s Make%sLoop();\n\n' %
- (self._LoopType(), self._gain_schedule_name))
-
- fd.write(self._namespace_end)
- fd.write('\n\n')
- fd.write("#endif // %s\n" % header_guard)
-
- def WriteCC(self, header_file_name, cc_file):
- """Writes the cc file to the file named cc_file."""
- with open(cc_file, 'w') as fd:
- fd.write('#include \"%s/control_loops/%s\"\n' %
- (self._TopDirectory(), header_file_name))
- fd.write('\n')
- fd.write('#include <vector>\n')
- fd.write('\n')
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- fd.write('\n')
- fd.write(self._namespace_start)
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlant())
- fd.write('\n')
-
- for loop in self._loops:
- fd.write(loop.DumpController())
- fd.write('\n')
-
- fd.write('%s Make%sPlant() {\n' %
- (self._PlantType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._CoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._CoeffType(), self._CoeffType(),
- loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
- fd.write('}\n\n')
-
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerType(), self._ControllerType(),
- loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._LoopType())
- fd.write('}\n\n')
-
- fd.write(self._namespace_end)
- fd.write('\n')
-
-
-class ControlLoop(object):
- def __init__(self, name):
- """Constructs a control loop object.
-
- Args:
- name: string, The name of the loop to use when writing the C++ files.
- """
- self._name = name
-
- def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
- """Calculates the discrete time values for A and B.
-
- Args:
- A_continuous: numpy.matrix, The continuous time A matrix
- B_continuous: numpy.matrix, The continuous time B matrix
- dt: float, The time step of the control loop
-
- Returns:
- (A, B), numpy.matrix, the control matricies.
- """
- return controls.c2d(A_continuous, B_continuous, dt)
-
- def InitializeState(self):
- """Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
- self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
-
- def PlaceControllerPoles(self, poles):
- """Places the controller poles.
-
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.K = controls.dplace(self.A, self.B, poles)
-
- def PlaceObserverPoles(self, poles):
- """Places the observer poles.
-
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.L = controls.dplace(self.A.T, self.C.T, poles).T
-
- def Update(self, U):
- """Simulates one time step with the provided U."""
- #U = numpy.clip(U, self.U_min, self.U_max)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
-
- def PredictObserver(self, U):
- """Runs the predict step of the observer update."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
-
- def CorrectObserver(self, U):
- """Runs the correct step of the observer update."""
- self.X_hat += numpy.linalg.inv(self.A) * self.L * (
- self.Y - self.C * self.X_hat - self.D * U)
-
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- self.X_hat = (self.A * self.X_hat + self.B * U +
- self.L * (self.Y - self.C * self.X_hat - self.D * U))
-
- def _DumpMatrix(self, matrix_name, matrix):
- """Dumps the provided matrix into a variable called matrix_name.
-
- Args:
- matrix_name: string, The variable name to save the matrix to.
- matrix: The matrix to dump.
-
- Returns:
- string, The C++ commands required to populate a variable named matrix_name
- with the contents of matrix.
- """
- ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
- matrix.shape[0], matrix.shape[1], matrix_name)]
- first = True
- for x in xrange(matrix.shape[0]):
- for y in xrange(matrix.shape[1]):
- element = matrix[x, y]
- if first:
- ans.append(' %s << ' % matrix_name)
- first = False
- else:
- ans.append(', ')
- ans.append(str(element))
-
- ans.append(';\n')
- return ''.join(ans)
-
- def DumpPlantHeader(self):
- """Writes out a c++ header declaration which will create a Plant object.
-
- Returns:
- string, The header declaration for the function.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
- num_states, num_inputs, num_outputs, self._name)
-
- def DumpPlant(self):
- """Writes out a c++ function which will create a PlantCoefficients object.
-
- Returns:
- string, The function which will create the object.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
- ' Make%sPlantCoefficients() {\n' % (
- num_states, num_inputs, num_outputs, self._name)]
-
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('B', self.B))
- ans.append(self._DumpMatrix('C', self.C))
- ans.append(self._DumpMatrix('D', self.D))
- ans.append(self._DumpMatrix('U_max', self.U_max))
- ans.append(self._DumpMatrix('U_min', self.U_min))
-
- ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
- num_outputs))
- ans.append('}\n')
- return ''.join(ans)
-
- def PlantFunction(self):
- """Returns the name of the plant coefficient function."""
- return 'Make%sPlantCoefficients()' % self._name
-
- def ControllerFunction(self):
- """Returns the name of the controller function."""
- return 'Make%sController()' % self._name
-
- def DumpControllerHeader(self):
- """Writes out a c++ header declaration which will create a Controller object.
-
- Returns:
- string, The header declaration for the function.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())
-
- def DumpController(self):
- """Returns a c++ function which will create a Controller object.
-
- Returns:
- string, The function which will create the object.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())]
-
- ans.append(self._DumpMatrix('L', self.L))
- ans.append(self._DumpMatrix('K', self.K))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
-
- ans.append(' return StateFeedbackController<%d, %d, %d>'
- '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
- num_states, num_inputs, num_outputs, self._name))
- ans.append('}\n')
- return ''.join(ans)
diff --git a/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
index 8ed62d6..43b597f 100644
--- a/bot3/control_loops/python/drivetrain.py
+++ b/bot3/control_loops/python/drivetrain.py
@@ -232,7 +232,8 @@
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
- drivetrain_high_low, drivetrain_high_high])
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/bot3/control_loops/python/elevator.py b/bot3/control_loops/python/elevator.py
deleted file mode 100644
index 2d72ff2..0000000
--- a/bot3/control_loops/python/elevator.py
+++ /dev/null
@@ -1,247 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import controls
-import polytope
-import polydrivetrain
-import numpy
-import sys
-import matplotlib
-from matplotlib import pylab
-
-class Elevator(control_loop.ControlLoop):
- def __init__(self, name="Elevator", mass=None):
- super(Elevator, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Mass of the elevator
- # TODO(comran): Get actual value.
- self.mass = 13.0
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # 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 ratio
- # TODO(comran): Get actual value.
- self.G = (56.0 / 12.0) * (84.0 / 14.0)
- # Pulley diameter
- # TODO(comran): Get actual value.
- self.r = 32 * 0.005 / numpy.pi / 2.0
- # Control loop time step
- self.dt = 0.005
-
- # Elevator spring constant (N/m)
- # TODO(comran): Get actual value.
- self.spring = 800.0
-
- # State is [average position, average velocity]
- # Input is [V]
-
- # TODO(comran): Change everything below.
-
- C1 = self.spring / (self.mass * 0.5)
- C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
- C3 = self.G * self.G * self.Kt / (
- self.R * self.r * self.r * self.mass * 0.5 * self.Kv)
-
- self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -C3, 0, 0],
- [0, 0, 0, 1],
- [0, 0, -C1 * 2.0, -C3]])
-
- print "Full speed is", C2 / C3 * 12.0
-
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [C2 / 2.0, C2 / 2.0],
- [0, 0],
- [C2 / 2.0, -C2 / 2.0]])
-
- self.C = numpy.matrix([[1, 0, 1, 0],
- [1, 0, -1, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
-
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
-
- print self.A
-
- controlability = controls.ctrb(self.A, self.B);
- print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(
- controlability)
-
- q_pos = 0.02
- q_vel = 0.400
- q_pos_diff = 0.01
- q_vel_diff = 0.45
- 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_diff ** 2.0)), 0.0],
- [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
-
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, 1.0 / (12.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
-
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
-
- self.rpl = 0.20
- self.ipl = 0.05
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- self.InitializeState()
-
-
-def CapU(U):
- if U[0, 0] - U[1, 0] > 24:
- return numpy.matrix([[12], [-12]])
- elif U[0, 0] - U[1, 0] < -24:
- return numpy.matrix([[-12], [12]])
- else:
- max_u = max(U[0, 0], U[1, 0])
- min_u = min(U[0, 0], U[1, 0])
- if max_u > 12:
- return U - (max_u - 12)
- if min_u < -12:
- return U - (min_u + 12)
- return U
-
-
-def run_test(elevator, initial_X, goal, max_separation_error=0.01,
- show_graph=True, iterations=200, controller_elevator=None,
- observer_elevator=None):
- """Runs the elevator plant with an initial condition and goal.
-
- The tests themselves are not terribly sophisticated; I just test for
- whether the goal has been reached and whether the separation goes
- outside of the initial and goal values by more than max_separation_error.
- Prints out something for a failure of either condition and returns
- False if tests fail.
- Args:
- elevator: elevator object to use.
- initial_X: starting state.
- goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for.
- controller_elevator: elevator object to get K from, or None if we should
- use elevator.
- observer_elevator: elevator object to use for the observer, or None if we
- should use the actual state.
- """
-
- elevator.X = initial_X
-
- if controller_elevator is None:
- controller_elevator = elevator
-
- if observer_elevator is not None:
- observer_elevator.X_hat = initial_X + 0.01
- observer_elevator.X_hat = initial_X
-
- # Various lists for graphing things.
- t = []
- x_avg = []
- x_sep = []
- x_hat_avg = []
- x_hat_sep = []
- v_avg = []
- v_sep = []
- u_left = []
- u_right = []
-
- sep_plot_gain = 100.0
-
- for i in xrange(iterations):
- X_hat = elevator.X
- if observer_elevator is not None:
- X_hat = observer_elevator.X_hat
- x_hat_avg.append(observer_elevator.X_hat[0, 0])
- x_hat_sep.append(observer_elevator.X_hat[2, 0] * sep_plot_gain)
- U = controller_elevator.K * (goal - X_hat)
- U = CapU(U)
- x_avg.append(elevator.X[0, 0])
- v_avg.append(elevator.X[1, 0])
- x_sep.append(elevator.X[2, 0] * sep_plot_gain)
- v_sep.append(elevator.X[3, 0])
- if observer_elevator is not None:
- observer_elevator.PredictObserver(U)
- elevator.Update(U)
- if observer_elevator is not None:
- observer_elevator.Y = elevator.Y
- observer_elevator.CorrectObserver(U)
-
- t.append(i * elevator.dt)
- u_left.append(U[0, 0])
- u_right.append(U[1, 0])
-
- print numpy.linalg.inv(elevator.A)
- print "delta time is ", elevator.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
-
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x_avg, label='x avg')
- pylab.plot(t, x_sep, label='x sep')
- if observer_elevator is not None:
- pylab.plot(t, x_hat_avg, label='x_hat avg')
- pylab.plot(t, x_hat_sep, label='x_hat sep')
- pylab.legend()
-
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u_left, label='u left')
- pylab.plot(t, u_right, label='u right')
- pylab.legend()
- pylab.show()
-
-
-def main(argv):
- loaded_mass = 25
- #loaded_mass = 0
- elevator = Elevator(mass=13 + loaded_mass)
- elevator_controller = Elevator(mass=13 + 15)
- observer_elevator = Elevator(mass=13 + 15)
- #observer_elevator = None
-
- # Test moving the elevator with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
- #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(elevator, initial_X, R, controller_elevator=elevator_controller,
- observer_elevator=observer_elevator)
-
- # Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name for the elevator."
- else:
- elevator = Elevator("Elevator")
- loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/elevator3.py b/bot3/control_loops/python/elevator3.py
index 120e8cc..04edd53 100755
--- a/bot3/control_loops/python/elevator3.py
+++ b/bot3/control_loops/python/elevator3.py
@@ -259,15 +259,24 @@
scenario_plotter.Plot()
# Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name for the Elevator."
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name for the Elevator and integral elevator."
else:
elevator = Elevator("Elevator")
- loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
+ loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator],
+ namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
loop_writer.Write(argv[1], argv[2])
+ integral_elevator = IntegralElevator("IntegralElevator")
+ integral_loop_writer = control_loop.ControlLoopWriter("IntegralElevator", [integral_elevator],
+ namespaces=['bot3', 'control_loops'])
+ if argv[3][-3:] == '.cc':
+ integral_loop_writer.Write(argv[4], argv[3])
+ else:
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/polydrivetrain.py b/bot3/control_loops/python/polydrivetrain.py
index a62c8c8..1090c8f 100644
--- a/bot3/control_loops/python/polydrivetrain.py
+++ b/bot3/control_loops/python/polydrivetrain.py
@@ -405,7 +405,8 @@
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ vdrivetrain.drivetrain_high_high],
+ namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
@@ -413,7 +414,8 @@
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ "CIM", [drivetrain.CIM()],
+ namespaces=['bot3', 'control_loops'])
if argv[5][-3:] == '.cc':
cim_writer.Write(argv[6], argv[5])
diff --git a/bot3/control_loops/update_elevator.sh b/bot3/control_loops/update_elevator.sh
new file mode 100644
index 0000000..8bfb0ba
--- /dev/null
+++ b/bot3/control_loops/update_elevator.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the elevator controllers.
+
+cd $(dirname $0)
+
+./python/elevator3.py elevator/elevator_motor_plant.h \
+ elevator/elevator_motor_plant.cc