| #include "frc971/control_loops/StateFeedbackLoop.h" |
| |
| typedef StateFeedbackLoop<4, 2> MatrixClass; |
| #define MATRIX_INIT A << 1.0000000000, 0.0095410093, 0.0000000000, -0.0000167223, 0.0000000000, 0.9096302600, 0.0000000000, -0.0032396985, 0.0000000000, -0.0000167223, 1.0000000000, 0.0095410093, 0.0000000000, -0.0032396985, 0.0000000000, 0.9096302600; \ |
| B << 0.0000628338, 0.0000022892, 0.0123712263, 0.0004435007, 0.0000022892, 0.0000628338, 0.0004435007, 0.0123712263; \ |
| C << 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000, 0.0000000000; \ |
| D << 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000; \ |
| L << 1.7496302600, -0.0032396985, 72.1296388532, -0.4369906587, -0.0032396985, 1.7496302600, -0.4369906586, 72.1296388532; \ |
| K << 242.8102455120, 19.7898401032, -8.7045950610, -0.9720464423, -8.7045950610, -0.9720464423, 242.8102455120, 19.7898401032; \ |
| U_max << 12.0000000000, 12.0000000000; \ |
| U_min << -12.0000000000, -12.0000000000; \ |
| |