Split the accelerating and decelerating gains for the superstructure.
Change-Id: Iae2c6d994a35443d0bc26fc5b811247ff2c7bf8b
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 64f8cd4..a51564b 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -444,9 +444,12 @@
number_of_states, number_of_inputs, number_of_outputs>>> controllers_;
// These are accessible from non-templated subclasses.
- static const int kNumStates = number_of_states;
- static const int kNumOutputs = number_of_outputs;
- static const int kNumInputs = number_of_inputs;
+ static constexpr int kNumStates = number_of_states;
+ static constexpr int kNumOutputs = number_of_outputs;
+ static constexpr int kNumInputs = number_of_inputs;
+
+ // Portion of U which is based on the feed-forwards.
+ Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
private:
// Internal state estimate.
@@ -459,8 +462,6 @@
Eigen::Matrix<double, number_of_inputs, 1> U_;
// Computed output before being capped.
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
- // Portion of U which is based on the feed-forwards.
- Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
int controller_index_;
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 9e12b6b..70740d5 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -24,10 +24,11 @@
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm"):
+ def __init__(self, name="Arm", J=None):
super(Arm, self).__init__(name=name)
- self._shoulder = Shoulder(name=name)
+ self._shoulder = Shoulder(name=name, J=J)
self._shooter = Wrist(name=name)
+ self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
# Do a coordinate transformation.
# X_shooter_grounded = X_shooter + X_shoulder
@@ -94,7 +95,10 @@
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
glog.debug('Shoulder K')
- glog.debug(self._shoulder.K)
+ glog.debug(repr(self._shoulder.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self._shoulder.A -
+ self._shoulder.B * self._shoulder.K)[0]))
# Compute controller gains.
# self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
@@ -135,8 +139,8 @@
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm"):
- super(IntegralArm, self).__init__(name=name)
+ def __init__(self, name="IntegralArm", J=None):
+ super(IntegralArm, self).__init__(name=name, J=J)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
@@ -154,11 +158,11 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
- q_pos_shoulder = 0.08
- q_vel_shoulder = 4.00
- q_voltage_shoulder = 3.0
+ q_pos_shoulder = 0.10
+ q_vel_shoulder = 0.005
+ q_voltage_shoulder = 3.5
q_pos_shooter = 0.08
- q_vel_shooter = 4.00
+ q_vel_shooter = 2.00
q_voltage_shooter = 1.0
self.Q = numpy.matrix(numpy.zeros((6, 6)))
self.Q[0, 0] = q_pos_shoulder ** 2.0
@@ -236,7 +240,7 @@
goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
shoulder_profile = TrapezoidProfile(arm.dt)
- shoulder_profile.set_maximum_acceleration(50.0)
+ shoulder_profile.set_maximum_acceleration(12.0)
shoulder_profile.set_maximum_velocity(10.0)
shoulder_profile.SetGoal(goal[0, 0])
shooter_profile = TrapezoidProfile(arm.dt)
@@ -318,8 +322,6 @@
self.u_shoulder.append(U[0, 0])
self.u_shooter.append(U[1, 0])
- glog.debug('Time: %f', self.t[-1])
-
ff_U -= U_uncapped - U
goal = controller.A * goal + controller.B * ff_U
@@ -378,7 +380,7 @@
scenario_plotter = ScenarioPlotter()
arm = Arm()
- arm_controller = IntegralArm()
+ arm_controller = IntegralArm(name='AcceleratingIntegralArm', J=12)
arm_observer = IntegralArm()
# Test moving the shoulder with constant separation.
@@ -406,8 +408,12 @@
namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
+ decelerating_arm_controller = IntegralArm(name='DeceleratingIntegralArm', J=5)
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralArm', [arm_controller], namespaces=namespaces)
+ 'IntegralArm', [arm_controller, decelerating_arm_controller],
+ namespaces=namespaces)
+ integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
+ arm_controller.shoulder_Kv))
integral_loop_writer.Write(argv[3], argv[4])
if FLAGS.plot:
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index 63cc0f4..da45bd1 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -42,7 +42,7 @@
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
- self.J = 0.34 + 0.1
+ self.J = 0.34 + 0.6
# Control loop time step
self.dt = 0.005
@@ -138,11 +138,12 @@
self.C = numpy.matrix(numpy.zeros((1, 3)))
self.C[0:1, 0:2] = self.C_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
q_pos = 0.12
q_vel = 2.00
- q_voltage = 3.0
+ q_voltage = 4.0
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
[0.0, 0.0, (q_voltage ** 2.0)]])
@@ -235,7 +236,10 @@
self.v.append(intake.X[1, 0])
self.a.append((self.v[-1] - last_v) / intake.dt)
- intake.Update(U + 0.0)
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ intake.Update(U + offset)
observer_intake.PredictObserver(U)
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 07a67f6..6beac61 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -17,7 +17,7 @@
pass
class Shoulder(control_loop.ControlLoop):
- def __init__(self, name="Shoulder", mass=None):
+ def __init__(self, name="Shoulder", J=None):
super(Shoulder, self).__init__(name)
# TODO(constants): Update all of these & retune poles.
# Stall Torque in N m
@@ -39,7 +39,10 @@
# Gear ratio
self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (42.0 / 12.0)
- self.J = 3.0
+ if J is None:
+ self.J = 10.0
+ else:
+ self.J = J
# Control loop time step
self.dt = 0.005
@@ -67,8 +70,8 @@
controllability = controls.ctrb(self.A, self.B)
- q_pos = 0.14
- q_vel = 4.5
+ q_pos = 0.16
+ q_vel = 1.5
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 4d4f5f1..660e577 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -39,7 +39,7 @@
# Gear ratio
self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
- self.J = 0.15
+ self.J = 0.20
# Control loop time step
self.dt = 0.005
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_;