Tune the flywheel and accelerator
Change-Id: I04530b7b5e0565f4dfe6d2978d1beb259f1fb0af
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index bb50d9d..bb6871c 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -21,19 +21,19 @@
G = (30.0 / 40.0) * numpy.power(G_per_wheel, 3.0)
# Overall flywheel inertia.
J = J_wheel * (
- 1.0 + numpy.power(G, -2.0) + numpy.power(G, -4.0) + numpy.power(G, -6.0))
+ 1.0 + numpy.power(G_per_wheel, -2.0) + numpy.power(G_per_wheel, -4.0) + numpy.power(G_per_wheel, -6.0))
# The position and velocity are measured for the final wheel.
kAccelerator = flywheel.FlywheelParams(
name='Accelerator',
motor=control_loop.Falcon(),
G=G,
- J=J + 0.0015,
+ J=J * 1.3,
q_pos=0.01,
q_vel=40.0,
- q_voltage=2.0,
- r_pos=0.05,
- controller_poles=[.86])
+ q_voltage=1.0,
+ r_pos=0.03,
+ controller_poles=[.89])
def main(argv):
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 035586a..35ff48f 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -17,21 +17,30 @@
# 40 tooth on the flywheel
# 48 for the falcon.
# 60 tooth on the outer wheel.
-G = 48.0 / 40.0
+G = 44.0 / 40.0
# Overall flywheel inertia.
J = 0.00507464
+J = 0.008
+
+def AddResistance(motor, resistance):
+ motor.resistance += resistance
+ return motor
+
+def ScaleKv(motor, scale):
+ motor.Kv *= scale
+ return motor
# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
- motor=control_loop.NMotor(control_loop.Falcon(), 2),
+ motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.01),
G=G,
J=J,
q_pos=0.01,
- q_vel=100.0,
- q_voltage=6.0,
- r_pos=0.05,
- controller_poles=[.90])
+ q_vel=10.0,
+ q_voltage=4.0,
+ r_pos=0.01,
+ controller_poles=[.89])
def main(argv):
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index a8921d9..d94573b 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -27,6 +27,9 @@
bemf_(bemf),
resistance_(resistance) {}
+ double resistance() const { return resistance_; }
+ double bemf() const { return bemf_; }
+
void CapU() override {
const double bemf_voltage = X_hat(1) / bemf_;
// Solve the system of equations:
@@ -38,7 +41,7 @@
// And we have a quadratic!
const double a = 1;
const double b = -bemf_voltage;
- const double c = -50.0 * 12.0 * resistance_;
+ const double c = -60.0 * 12.0 * resistance_;
// Root is always positive.
const double root = std::sqrt(b * b - 4.0 * a * c);
@@ -100,6 +103,11 @@
double FlywheelController::voltage() const { return loop_->U(0, 0); }
+double FlywheelController::current() const {
+ return ((voltage() - (velocity() / loop_->bemf())) / (loop_->resistance())) *
+ voltage() / 12.0;
+}
+
void FlywheelController::Update(bool disabled) {
loop_->mutable_R() = loop_->next_R();
if (loop_->R(1, 0) < 1.0) {
@@ -116,6 +124,10 @@
const int oldest_history_position = history_position_;
const int newest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+ const int second_newest_history_position =
+ ((newest_history_position == 0) ? kHistoryLength
+ : newest_history_position) -
+ 1;
const double total_loop_time = ::aos::time::DurationInSeconds(
std::get<1>(history_[newest_history_position]) -
@@ -125,18 +137,32 @@
std::get<0>(history_[newest_history_position]) -
std::get<0>(history_[oldest_history_position]);
+ const double last_loop_time = ::aos::time::DurationInSeconds(
+ std::get<1>(history_[newest_history_position]) -
+ std::get<1>(history_[second_newest_history_position]));
+
+ const double last_distance_traveled =
+ std::get<0>(history_[newest_history_position]) -
+ std::get<0>(history_[second_newest_history_position]);
+
// Compute the distance moved over that time period.
avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
FlywheelControllerStatusBuilder builder(*fbb);
builder.add_avg_angular_velocity(avg_angular_velocity_);
+ builder.add_dt_angular_velocity(last_distance_traveled / last_loop_time);
builder.add_angular_velocity(loop_->X_hat(1, 0));
builder.add_voltage_error(loop_->X_hat(2, 0));
+ builder.add_commanded_current(current());
builder.add_angular_velocity_goal(last_goal_);
return builder.Finish();
}
+FlywheelController::~FlywheelController() {}
+
+double FlywheelController::velocity() const { return loop_->X_hat(1, 0); }
+
} // namespace shooter
} // namespace superstructure
} // namespace control_loops
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index d5f7ede..5fdac39 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -15,6 +15,8 @@
namespace superstructure {
namespace shooter {
+class CurrentLimitedStateFeedbackController;
+
// Handles the velocity control of each flywheel.
class FlywheelController {
public:
@@ -23,6 +25,8 @@
HybridKalman<3, 1, 1>> &&loop,
double bemf, double resistance);
+ ~FlywheelController();
+
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
// Sets the current encoder position in radians
@@ -36,8 +40,11 @@
// Returns the control loop calculated voltage.
double voltage() const;
+ // Returns the expected battery current for the last U.
+ double current() const;
+
// Returns the instantaneous velocity.
- double velocity() const { return loop_->X_hat(1, 0); }
+ double velocity() const;
// Executes the control loop for a cycle.
void Update(bool disabled);
@@ -48,10 +55,7 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<
- StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>>>
- loop_;
+ ::std::unique_ptr<CurrentLimitedStateFeedbackController> loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 8cdcf8c..9db79b0 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -16,8 +16,11 @@
} // namespace
Shooter::Shooter()
- : finisher_(finisher::MakeIntegralFinisherLoop(), finisher::kBemf,
- finisher::kResistance),
+ : finisher_(
+ finisher::MakeIntegralFinisherLoop(), finisher::kBemf,
+ // There are 2 motors. So the current limit per motor is going to be
+ // using resistance * 2 to un-parallel the motor resistances.
+ finisher::kResistance * 2.0),
accelerator_left_(accelerator::MakeIntegralAcceleratorLoop(),
accelerator::kBemf, accelerator::kResistance),
accelerator_right_(accelerator::MakeIntegralAcceleratorLoop(),
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index d6f5532..7b2b8ad 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -286,7 +286,7 @@
// Confirm that we aren't drawing too much current.
CHECK_NEAR(accelerator_left_plant_->battery_current(accelerator_left_U),
- 0.0, 60.0);
+ 0.0, 70.0);
::Eigen::Matrix<double, 1, 1> accelerator_right_U;
accelerator_right_U
@@ -295,14 +295,15 @@
// Confirm that we aren't drawing too much current.
CHECK_NEAR(accelerator_right_plant_->battery_current(accelerator_right_U),
- 0.0, 60.0);
+ 0.0, 70.0);
::Eigen::Matrix<double, 1, 1> finisher_U;
finisher_U << superstructure_output_fetcher_->finisher_voltage() +
finisher_plant_->voltage_offset();
- // Confirm that we aren't drawing too much current.
- CHECK_NEAR(finisher_plant_->battery_current(finisher_U), 0.0, 60.0);
+ // Confirm that we aren't drawing too much current. 2 motors -> twice the
+ // lumped current since our model can't tell them apart.
+ CHECK_NEAR(finisher_plant_->battery_current(finisher_U), 0.0, 200.0);
hood_plant_->Update(hood_U);
intake_plant_->Update(intake_U);
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 83ee607..adca326 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -17,6 +17,12 @@
// Voltage error.
voltage_error:double (id: 3);
+
+ // The commanded battery current.
+ commanded_current:double (id: 4);
+
+ // The angular velocity of the flywheel computed using delta x / delta t
+ dt_angular_velocity:double (id: 5);
}
table ShooterStatus {