Split the accelerating and decelerating gains for the superstructure.
Change-Id: Iae2c6d994a35443d0bc26fc5b811247ff2c7bf8b
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 8147fc2..c64fac1 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -631,6 +631,8 @@
status->intake.estimator_state = intake_.IntakeEstimatorState();
status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
+ status->shoulder_controller_index = arm_.controller_index();
+
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
last_intake_angle_ = intake_.angle();
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 277e3ce..0883446 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -81,6 +81,8 @@
JointState intake;
JointState shoulder;
JointState wrist;
+
+ int32_t shoulder_controller_index;
};
message Position {
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index b2d8aa7..0a0a825 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -162,7 +162,7 @@
}
Arm::Arm()
- : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>(
+ : loop_(new ArmControlLoop(
::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
shoulder_profile_(::aos::controls::kLoopFrequency),
wrist_profile_(::aos::controls::kLoopFrequency),
@@ -338,19 +338,14 @@
void Arm::Update(bool disable) {
if (!disable) {
// Compute next goal.
- ::Eigen::Matrix<double, 2, 1> goal_state_shoulder =
- shoulder_profile_.Update(unprofiled_goal_(0, 0),
- unprofiled_goal_(1, 0));
- loop_->mutable_next_R(0, 0) = goal_state_shoulder(0, 0);
- loop_->mutable_next_R(1, 0) = goal_state_shoulder(1, 0);
+ loop_->mutable_next_R().block<2, 1>(0, 0) = shoulder_profile_.Update(
+ unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
- ::Eigen::Matrix<double, 2, 1> goal_state_wrist =
+ loop_->mutable_next_R().block<2, 1>(2, 0) =
wrist_profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
- loop_->mutable_next_R(2, 0) = goal_state_wrist(0, 0);
- loop_->mutable_next_R(3, 0) = goal_state_wrist(1, 0);
- loop_->mutable_next_R(4, 0) = unprofiled_goal_(4, 0);
- loop_->mutable_next_R(5, 0) = unprofiled_goal_(5, 0);
+ loop_->mutable_next_R().block<2, 1>(4, 0) =
+ unprofiled_goal_.block<2, 1>(4, 0);
CapGoal("next R", &loop_->mutable_next_R());
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index b8c674f..b4fbead 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -10,6 +10,7 @@
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/integral_arm_plant.h"
namespace y2016 {
namespace control_loops {
@@ -105,6 +106,53 @@
bool zeroed_ = false;
};
+class ArmControlLoop
+ : public ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2> {
+ public:
+ ArmControlLoop(SimpleCappedStateFeedbackLoop<6, 2, 2> &&loop)
+ : SimpleCappedStateFeedbackLoop<6, 2, 2>(::std::move(loop)) {}
+
+ const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
+ const Eigen::Matrix<double, 2, 1> accelerating_ff =
+ controller(0).Kff * (next_R() - controller(0).plant.A() * R());
+ const Eigen::Matrix<double, 2, 1> accelerating_controller =
+ controller(0).K * error() + accelerating_ff;
+
+ const Eigen::Matrix<double, 2, 1> decelerating_ff =
+ controller(1).Kff * (next_R() - controller(1).plant.A() * R());
+ const Eigen::Matrix<double, 2, 1> decelerating_controller =
+ controller(1).K * error() + decelerating_ff;
+
+ const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
+ bool use_accelerating_controller = true;
+ LOG(DEBUG, "Accelerating at %f, decel %f, bemf %f\n",
+ accelerating_controller(0, 0), accelerating_controller(1, 0),
+ bemf_voltage);
+ if (IsAccelerating(bemf_voltage, accelerating_controller(0, 0))) {
+ use_accelerating_controller = true;
+ } else {
+ use_accelerating_controller = false;
+ }
+ if (use_accelerating_controller) {
+ ff_U_ = accelerating_ff;
+ set_controller_index(0);
+ return accelerating_controller;
+ } else {
+ set_controller_index(1);
+ ff_U_ = decelerating_ff;
+ return decelerating_controller;
+ }
+ }
+
+ private:
+ bool IsAccelerating(double bemf_voltage, double voltage) {
+ if (bemf_voltage > 0) {
+ return voltage > bemf_voltage;
+ } else {
+ return voltage < bemf_voltage;
+ }
+ }
+};
class Arm {
public:
@@ -130,6 +178,8 @@
void set_unprofiled_goal(double unprofiled_goal_shoulder,
double unprofiled_goal_wrist);
+ int controller_index() const { return loop_->controller_index(); }
+
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
@@ -196,7 +246,7 @@
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
::std::unique_ptr<
- ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>> loop_;
+ ArmControlLoop> loop_;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;