Autogen rules written for elevator module.
Put most things in the y2015 namespace since codegen wants to place
the controller gains in that namespace.
Change-Id: Ib3ef6eb38200bf0d80cba972cbe06ea366522ec6
diff --git a/y2015/control_loops/fridge/BUILD b/y2015/control_loops/fridge/BUILD
index 2e90a80..a9bb737 100644
--- a/y2015/control_loops/fridge/BUILD
+++ b/y2015/control_loops/fridge/BUILD
@@ -26,27 +26,52 @@
],
)
+genrule(
+ name = 'genrule_elevator',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015/control_loops/python:elevator) $(OUTS)',
+ tools = [
+ '//y2015/control_loops/python:elevator',
+ ],
+ outs = [
+ 'elevator_motor_plant.h',
+ 'elevator_motor_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'elevator_plants',
+ srcs = [
+ 'elevator_motor_plant.cc',
+ ],
+ hdrs = [
+ 'elevator_motor_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
cc_library(
name = 'fridge_lib',
srcs = [
'fridge.cc',
'integral_arm_plant.cc',
- 'elevator_motor_plant.cc',
],
hdrs = [
'fridge.h',
'integral_arm_plant.h',
- 'elevator_motor_plant.h',
],
deps = [
+ ':elevator_plants',
':fridge_queue',
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
- '//y2015:constants',
- '//frc971/control_loops:state_feedback_loop',
'//frc971/control_loops/voltage_cap:voltage_cap',
+ '//frc971/control_loops:state_feedback_loop',
'//frc971/zeroing',
'//y2015/util:kinematics',
+ '//y2015:constants',
],
)
diff --git a/y2015/control_loops/fridge/elevator_motor_plant.cc b/y2015/control_loops/fridge/elevator_motor_plant.cc
deleted file mode 100644
index 995d838..0000000
--- a/y2015/control_loops/fridge/elevator_motor_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015/control_loops/fridge/elevator_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeElevatorPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00435668193669, 0.0, 0.0, 0.0, 0.754212786054, 0.0, 0.0, 0.0, 0.0, 0.997194498569, 0.00435222083164, 0.0, 0.0, -1.07131589702, 0.751658962986;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.82580284276e-05, 3.82580284276e-05, 0.0146169286307, 0.0146169286307, 3.82387898839e-05, -3.82387898839e-05, 0.0146019613563, -0.0146019613563;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 1, 0, 1, 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 StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeElevatorController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 0.677106393027, 0.677106393027, 35.5375738607, 35.5375738607, 0.674426730777, -0.674426730777, 34.7138874344, -34.7138874344;
- Eigen::Matrix<double, 2, 4> K;
- K << 321.310606763, 11.7674534233, 601.047935717, 12.6977148843, 321.310606764, 11.7674534233, -601.047935716, -12.6977148843;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00577646258091, 0.0, 0.0, 0.0, 1.32588576923, 0.0, 0.0, 0.0, 0.0, 0.996613922337, -0.00577054766522, 0.0, 0.0, 1.42044250221, 1.32216599481;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeElevatorPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeElevatorPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeElevatorPlantCoefficients()));
- return StateFeedbackPlant<4, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeElevatorLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeElevatorController()));
- return StateFeedbackLoop<4, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2015/control_loops/fridge/elevator_motor_plant.h b/y2015/control_loops/fridge/elevator_motor_plant.h
deleted file mode 100644
index e68e6d7..0000000
--- a/y2015/control_loops/fridge/elevator_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
-#define Y2015_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeElevatorPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeElevatorController();
-
-StateFeedbackPlant<4, 2, 2> MakeElevatorPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeElevatorLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2015_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index 9f2241a..4c24a1f 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -12,8 +12,9 @@
#include "y2015/constants.h"
-namespace frc971 {
+namespace y2015 {
namespace control_loops {
+namespace fridge {
namespace chrono = ::std::chrono;
@@ -27,8 +28,9 @@
template <int S>
void CappedStateFeedbackLoop<S>::CapU() {
- VoltageCap(max_voltage_, this->U(0, 0), this->U(1, 0), &this->mutable_U(0, 0),
- &this->mutable_U(1, 0));
+ ::frc971::control_loops::VoltageCap(max_voltage_, this->U(0, 0),
+ this->U(1, 0), &this->mutable_U(0, 0),
+ &this->mutable_U(1, 0));
}
template <int S>
@@ -49,10 +51,10 @@
return deltaR;
}
-Fridge::Fridge(control_loops::FridgeQueue *fridge)
- : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
- arm_loop_(new CappedStateFeedbackLoop<5>(
- StateFeedbackLoop<5, 2, 2>(MakeIntegralArmLoop()))),
+Fridge::Fridge(FridgeQueue *fridge)
+ : aos::controls::ControlLoop<FridgeQueue>(fridge),
+ arm_loop_(new CappedStateFeedbackLoop<5>(StateFeedbackLoop<5, 2, 2>(
+ ::frc971::control_loops::MakeIntegralArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop<4>(
StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
@@ -239,10 +241,10 @@
return arm_zeroing_velocity_;
}
-void Fridge::RunIteration(const control_loops::FridgeQueue::Goal *unsafe_goal,
- const control_loops::FridgeQueue::Position *position,
- control_loops::FridgeQueue::Output *output,
- control_loops::FridgeQueue::Status *status) {
+void Fridge::RunIteration(const FridgeQueue::Goal *unsafe_goal,
+ const FridgeQueue::Position *position,
+ FridgeQueue::Output *output,
+ FridgeQueue::Status *status) {
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
left_elevator_estimator_.Reset();
@@ -709,17 +711,19 @@
status->grabbers.bottom_front = false;
status->grabbers.bottom_back = false;
}
- zeroing::PopulateEstimatorState(left_arm_estimator_, &status->left_arm_state);
- zeroing::PopulateEstimatorState(right_arm_estimator_,
- &status->right_arm_state);
- zeroing::PopulateEstimatorState(left_elevator_estimator_,
- &status->left_elevator_state);
- zeroing::PopulateEstimatorState(right_elevator_estimator_,
- &status->right_elevator_state);
+ ::frc971::zeroing::PopulateEstimatorState(left_arm_estimator_,
+ &status->left_arm_state);
+ ::frc971::zeroing::PopulateEstimatorState(right_arm_estimator_,
+ &status->right_arm_state);
+ ::frc971::zeroing::PopulateEstimatorState(left_elevator_estimator_,
+ &status->left_elevator_state);
+ ::frc971::zeroing::PopulateEstimatorState(right_elevator_estimator_,
+ &status->right_elevator_state);
status->estopped = (state_ == ESTOP);
status->state = state_;
last_state_ = state_;
}
+} // namespace fridge
} // namespace control_loops
-} // namespace frc971
+} // namespace y2015
diff --git a/y2015/control_loops/fridge/fridge.h b/y2015/control_loops/fridge/fridge.h
index 0448e09..ee64554 100644
--- a/y2015/control_loops/fridge/fridge.h
+++ b/y2015/control_loops/fridge/fridge.h
@@ -10,8 +10,9 @@
#include "frc971/zeroing/zeroing.h"
#include "y2015/util/kinematics.h"
-namespace frc971 {
+namespace y2015 {
namespace control_loops {
+namespace fridge {
namespace testing {
class FridgeTest_DisabledGoalTest_Test;
class FridgeTest_ArmGoalPositiveWindupTest_Test;
@@ -42,10 +43,10 @@
};
class Fridge
- : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
+ : public aos::controls::ControlLoop<FridgeQueue> {
public:
- explicit Fridge(
- control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
+ explicit Fridge(FridgeQueue *fridge_queue =
+ &::y2015::control_loops::fridge::fridge_queue);
enum State {
// Waiting to receive data before doing anything.
@@ -72,10 +73,10 @@
State state() const { return state_; }
protected:
- void RunIteration(const control_loops::FridgeQueue::Goal *goal,
- const control_loops::FridgeQueue::Position *position,
- control_loops::FridgeQueue::Output *output,
- control_loops::FridgeQueue::Status *status) override;
+ void RunIteration(const FridgeQueue::Goal *goal,
+ const FridgeQueue::Position *position,
+ FridgeQueue::Output *output,
+ FridgeQueue::Status *status) override;
private:
friend class testing::FridgeTest_DisabledGoalTest_Test;
@@ -127,10 +128,10 @@
::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
- zeroing::ZeroingEstimator left_arm_estimator_;
- zeroing::ZeroingEstimator right_arm_estimator_;
- zeroing::ZeroingEstimator left_elevator_estimator_;
- zeroing::ZeroingEstimator right_elevator_estimator_;
+ ::frc971::zeroing::ZeroingEstimator left_arm_estimator_;
+ ::frc971::zeroing::ZeroingEstimator right_arm_estimator_;
+ ::frc971::zeroing::ZeroingEstimator left_elevator_estimator_;
+ ::frc971::zeroing::ZeroingEstimator right_elevator_estimator_;
// Offsets from the encoder position to the absolute position. Add these to
// the encoder position to get the absolute position.
@@ -153,7 +154,7 @@
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
- control_loops::FridgeQueue::Position current_position_;
+ FridgeQueue::Position current_position_;
ProfilingType last_profiling_type_;
aos::util::ElevatorArmKinematics kinematics_;
@@ -165,8 +166,9 @@
aos::util::TrapezoidProfile y_profile_;
};
+} // namespace fridge
} // namespace control_loops
-} // namespace frc971
+} // namespace y2015
#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
diff --git a/y2015/control_loops/fridge/fridge.q b/y2015/control_loops/fridge/fridge.q
index 257374d..9df30e3 100644
--- a/y2015/control_loops/fridge/fridge.q
+++ b/y2015/control_loops/fridge/fridge.q
@@ -1,4 +1,4 @@
-package frc971.control_loops;
+package y2015.control_loops.fridge;
import "aos/common/controls/control_loops.q";
import "frc971/control_loops/control_loops.q";
@@ -82,8 +82,8 @@
};
message Position {
- PotAndIndexPair arm;
- PotAndIndexPair elevator;
+ .frc971.PotAndIndexPair arm;
+ .frc971.PotAndIndexPair elevator;
};
message Status {
@@ -128,10 +128,10 @@
// The internal state of the state machine.
int32_t state;
- EstimatorState left_elevator_state;
- EstimatorState right_elevator_state;
- EstimatorState left_arm_state;
- EstimatorState right_arm_state;
+ .frc971.EstimatorState left_elevator_state;
+ .frc971.EstimatorState right_elevator_state;
+ .frc971.EstimatorState left_arm_state;
+ .frc971.EstimatorState right_arm_state;
};
message Output {
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
index 7719fb4..2378622 100644
--- a/y2015/control_loops/fridge/fridge_lib_test.cc
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -20,8 +20,9 @@
using ::aos::time::Time;
-namespace frc971 {
+namespace y2015 {
namespace control_loops {
+namespace fridge {
namespace testing {
// Class which simulates the fridge and sends out queue messages with the
// position.
@@ -30,7 +31,8 @@
static constexpr double kNoiseScalar = 0.1;
// Constructs a simulation.
FridgeSimulation()
- : arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
+ : arm_plant_(new StateFeedbackPlant<4, 2, 2>(
+ ::frc971::control_loops::MakeArmPlant())),
elevator_plant_(new StateFeedbackPlant<4, 2, 2>(MakeElevatorPlant())),
left_arm_pot_encoder_(
constants::GetValues().fridge.left_arm_zeroing.index_difference),
@@ -40,11 +42,11 @@
constants::GetValues().fridge.left_elev_zeroing.index_difference),
right_elevator_pot_encoder_(
constants::GetValues().fridge.right_elev_zeroing.index_difference),
- fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
- ".frc971.control_loops.fridge_queue.goal",
- ".frc971.control_loops.fridge_queue.position",
- ".frc971.control_loops.fridge_queue.output",
- ".frc971.control_loops.fridge_queue.status") {
+ fridge_queue_(".y2015.control_loops.fridge.fridge_queue", 0xe4e05855,
+ ".y2015.control_loops.fridge.fridge_queue.goal",
+ ".y2015.control_loops.fridge.fridge_queue.position",
+ ".y2015.control_loops.fridge.fridge_queue.output",
+ ".y2015.control_loops.fridge.fridge_queue.status") {
// Initialize the elevator.
InitializeElevatorPosition(
constants::GetValues().fridge.elevator.lower_limit);
@@ -109,7 +111,7 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::FridgeQueue::Position> position =
+ ::aos::ScopedMessagePtr<FridgeQueue::Position> position =
fridge_queue_.position.MakeMessage();
left_arm_pot_encoder_.GetSensorValues(&position->arm.left);
@@ -196,10 +198,10 @@
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> arm_plant_;
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> elevator_plant_;
- PositionSensorSimulator left_arm_pot_encoder_;
- PositionSensorSimulator right_arm_pot_encoder_;
- PositionSensorSimulator left_elevator_pot_encoder_;
- PositionSensorSimulator right_elevator_pot_encoder_;
+ ::frc971::control_loops::PositionSensorSimulator left_arm_pot_encoder_;
+ ::frc971::control_loops::PositionSensorSimulator right_arm_pot_encoder_;
+ ::frc971::control_loops::PositionSensorSimulator left_elevator_pot_encoder_;
+ ::frc971::control_loops::PositionSensorSimulator right_elevator_pot_encoder_;
FridgeQueue fridge_queue_;
@@ -211,11 +213,11 @@
class FridgeTest : public ::aos::testing::ControlLoopTest {
protected:
FridgeTest()
- : fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
- ".frc971.control_loops.fridge_queue.goal",
- ".frc971.control_loops.fridge_queue.position",
- ".frc971.control_loops.fridge_queue.output",
- ".frc971.control_loops.fridge_queue.status"),
+ : fridge_queue_(".y2015.control_loops.fridge.fridge_queue", 0xe4e05855,
+ ".y2015.control_loops.fridge.fridge_queue.goal",
+ ".y2015.control_loops.fridge.fridge_queue.position",
+ ".y2015.control_loops.fridge.fridge_queue.output",
+ ".y2015.control_loops.fridge.fridge_queue.status"),
fridge_(&fridge_queue_),
fridge_plant_(),
kinematics_(constants::GetValues().fridge.arm_length,
@@ -223,7 +225,7 @@
constants::GetValues().fridge.elevator.lower_limit,
constants::GetValues().fridge.arm.upper_limit,
constants::GetValues().fridge.arm.lower_limit) {
- set_team_id(kTeamNumber);
+ set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
void VerifyNearGoal() {
@@ -731,5 +733,6 @@
// after we are zeroed.
} // namespace testing
+} // namespace fridge
} // namespace control_loops
-} // namespace frc971
+} // namespace y2015
diff --git a/y2015/control_loops/fridge/fridge_main.cc b/y2015/control_loops/fridge/fridge_main.cc
index e0fd6ea..658a4af 100644
--- a/y2015/control_loops/fridge/fridge_main.cc
+++ b/y2015/control_loops/fridge/fridge_main.cc
@@ -4,7 +4,7 @@
int main() {
::aos::Init();
- frc971::control_loops::Fridge fridge;
+ y2015::control_loops::fridge::Fridge fridge;
fridge.Run();
::aos::Cleanup();
return 0;
diff --git a/y2015/control_loops/fridge/replay_fridge.cc b/y2015/control_loops/fridge/replay_fridge.cc
index 65cc98a..842aa0d 100644
--- a/y2015/control_loops/fridge/replay_fridge.cc
+++ b/y2015/control_loops/fridge/replay_fridge.cc
@@ -14,8 +14,9 @@
::aos::InitNRT();
- ::aos::controls::ControlLoopReplayer<::frc971::control_loops::FridgeQueue>
- replayer(&::frc971::control_loops::fridge_queue, "fridge");
+ ::aos::controls::ControlLoopReplayer<
+ ::y2015::control_loops::fridge::FridgeQueue>
+ replayer(&::y2015::control_loops::fridge::fridge_queue, "fridge");
for (int i = 1; i < argc; ++i) {
replayer.ProcessFile(argv[i]);
}