Merge "Add april tag corner display to viewer"
diff --git a/frc971/analysis/in_process_plotter.cc b/frc971/analysis/in_process_plotter.cc
index 33d999e..b327c54 100644
--- a/frc971/analysis/in_process_plotter.cc
+++ b/frc971/analysis/in_process_plotter.cc
@@ -80,7 +80,7 @@
void Plotter::AddLine(const std::vector<double> &x,
const std::vector<double> &y, LineOptions options) {
- CHECK_EQ(x.size(), y.size());
+ CHECK_EQ(x.size(), y.size()) << ": " << options.label;
CHECK(!position_.IsNull())
<< "You must call AddFigure() before calling AddLine().";
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index ee53c4a..a62b45f 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -354,6 +354,9 @@
"fixed_quadrature.h",
],
target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "@org_tuxfamily_eigen//:eigen",
+ ],
)
cc_test(
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.cc b/frc971/control_loops/double_jointed_arm/dynamics.cc
index b5b6c8d..2842ad4 100644
--- a/frc971/control_loops/double_jointed_arm/dynamics.cc
+++ b/frc971/control_loops/double_jointed_arm/dynamics.cc
@@ -9,29 +9,29 @@
Dynamics::Dynamics(ArmConstants arm_constants)
: arm_constants_(arm_constants),
- K3_((::Eigen::Matrix<double, 2, 2>() << arm_constants_.g1 *
+ K3_((::Eigen::Matrix<double, 2, 2>() << arm_constants_.g0 *
arm_constants_.Kt /
arm_constants_.resistance,
0.0, 0.0,
- arm_constants_.g2 * arm_constants_.num_distal_motors *
+ arm_constants_.g1 * arm_constants_.num_distal_motors *
arm_constants_.Kt / arm_constants_.resistance)
.finished()),
K3_inverse_(K3_.inverse()),
K4_((::Eigen::Matrix<double, 2, 2>()
- << arm_constants_.g1 * arm_constants_.g1 * arm_constants_.Kt /
+ << arm_constants_.g0 * arm_constants_.g0 * arm_constants_.Kt /
(arm_constants_.Kv * arm_constants_.resistance),
0.0, 0.0,
- arm_constants_.g2 * arm_constants_.g2 * arm_constants_.Kt *
+ arm_constants_.g1 * arm_constants_.g1 * arm_constants_.Kt *
arm_constants_.num_distal_motors /
(arm_constants_.Kv * arm_constants_.resistance))
.finished()),
- alpha_(arm_constants_.j1 +
- arm_constants_.r1 * arm_constants_.r1 * arm_constants_.m1 +
- arm_constants_.l1 * arm_constants_.l1 * arm_constants_.m2),
- beta_(arm_constants_.l1 * arm_constants_.r2 * arm_constants_.m2),
- gamma_(arm_constants_.j2 +
- arm_constants_.r2 * arm_constants_.r2 * arm_constants_.m2) {}
+ alpha_(arm_constants_.j0 +
+ arm_constants_.r0 * arm_constants_.r0 * arm_constants_.m0 +
+ arm_constants_.l0 * arm_constants_.l0 * arm_constants_.m1),
+ beta_(arm_constants_.l0 * arm_constants_.r1 * arm_constants_.m1),
+ gamma_(arm_constants_.j1 +
+ arm_constants_.r1 * arm_constants_.r1 * arm_constants_.m1) {}
} // namespace arm
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.h b/frc971/control_loops/double_jointed_arm/dynamics.h
index 1e934bc..d4de1ed 100644
--- a/frc971/control_loops/double_jointed_arm/dynamics.h
+++ b/frc971/control_loops/double_jointed_arm/dynamics.h
@@ -12,26 +12,26 @@
namespace arm {
struct ArmConstants {
- // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
+ // Below, 0 refers to the proximal joint, and 1 refers to the distal joint.
// Length of the joints in meters.
+ double l0;
double l1;
- double l2;
// Mass of the joints in kilograms.
+ double m0;
double m1;
- double m2;
// Moment of inertia of the joints in kg m^2
+ double j0;
double j1;
- double j2;
// Radius of the center of mass of the joints in meters.
+ double r0;
double r1;
- double r2;
// Gear ratios for the two joints.
+ double g0;
double g1;
- double g2;
// motor constants.
double efficiency_tweak;
@@ -48,6 +48,8 @@
// This class captures the dynamics of our system. It doesn't actually need to
// store state yet, so everything can be constexpr and/or static.
+//
+// 0, 0 is straight up.
class Dynamics {
public:
Dynamics(ArmConstants arm_constants);
@@ -55,9 +57,10 @@
// K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
// These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
// You probbaly want MatriciesForState.
- void NormilizedMatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) const {
+ void NormilizedMatriciesForState(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) const {
const double angle = X(0, 0) - X(2, 0);
const double s = ::std::sin(angle);
const double c = ::std::cos(angle);
@@ -75,6 +78,50 @@
(*K2_result)(0, 1) *= X(3, 0);
}
+ // Calculates the joint torques as a function of the state and command.
+ const ::Eigen::Matrix<double, 2, 1> TorqueFromCommand(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ return K3_ * U - K4_ * velocity;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> CurrentFromTorque(
+ const ::Eigen::Matrix<double, 2, 1> &torque) {
+ return ::Eigen::DiagonalMatrix<double, 2>(
+ 1.0 / (arm_constants_.Kt * arm_constants_.g0),
+ 1.0 / (arm_constants_.Kt * arm_constants_.g1 *
+ arm_constants_.num_distal_motors)) *
+ torque;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> CurrentFromCommand(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return CurrentFromTorque(TorqueFromCommand(X, U));
+ }
+
+ // Computes the two joint torques given the state and the external force in
+ // x, y.
+ const ::Eigen::Matrix<double, 2, 1> TorqueFromForce(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &F) {
+ const ::Eigen::Matrix<double, 2, 1> L0(std::sin(X(0)) * arm_constants_.l0,
+ std::cos(X(0)) * arm_constants_.l0);
+ const ::Eigen::Matrix<double, 2, 1> L1(std::sin(X(2)) * arm_constants_.l1,
+ std::cos(X(2)) * arm_constants_.l1);
+
+ const Eigen::Matrix<double, 2, 1> Fn1 =
+ F - L0.normalized().dot(F) * L0.normalized();
+
+ const double torque1 = L0.x() * Fn1.y() - L0.y() * Fn1.x();
+ const double torque2 = L1.x() * F.y() - L1.y() * F.x();
+
+ return ::Eigen::Matrix<double, 2, 1>(torque1, torque2);
+ }
+
// TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
// Calculates the acceleration given the current state and control input.
@@ -150,10 +197,10 @@
const ::Eigen::Matrix<double, 4, 1> &X) const {
const double accel_due_to_gravity = 9.8 * arm_constants_.efficiency_tweak;
return (::Eigen::Matrix<double, 2, 1>()
- << (arm_constants_.r1 * arm_constants_.m1 +
- arm_constants_.l1 * arm_constants_.m2) *
+ << (arm_constants_.r0 * arm_constants_.m0 +
+ arm_constants_.l0 * arm_constants_.m1) *
::std::sin(X(0)) * accel_due_to_gravity,
- arm_constants_.r2 * arm_constants_.m2 * ::std::sin(X(2)) *
+ arm_constants_.r1 * arm_constants_.m1 * ::std::sin(X(2)) *
accel_due_to_gravity)
.finished() *
(FLAGS_gravity ? 1.0 : 0.0);
@@ -178,11 +225,17 @@
const ArmConstants arm_constants_;
// K3, K4 matricies described above.
+ const ::Eigen::Matrix<double, 2, 2> &K3() const { return K3_; }
+ const ::Eigen::Matrix<double, 2, 2> &K3_inverse() const {
+ return K3_inverse_;
+ }
+ const ::Eigen::Matrix<double, 2, 2> &K4() const { return K4_; }
+
+ private:
const ::Eigen::Matrix<double, 2, 2> K3_;
const ::Eigen::Matrix<double, 2, 2> K3_inverse_;
const ::Eigen::Matrix<double, 2, 2> K4_;
- private:
const double alpha_;
const double beta_;
const double gamma_;
diff --git a/frc971/control_loops/double_jointed_arm/test_constants.h b/frc971/control_loops/double_jointed_arm/test_constants.h
index ea1d3b7..4885d14 100644
--- a/frc971/control_loops/double_jointed_arm/test_constants.h
+++ b/frc971/control_loops/double_jointed_arm/test_constants.h
@@ -14,22 +14,22 @@
constexpr double kStallCurrent = 89.0;
constexpr ArmConstants kArmConstants = {
- .l1 = 46.25 * 0.0254,
- .l2 = 41.80 * 0.0254,
- .m1 = 9.34 / 2.2,
- .m2 = 9.77 / 2.2,
+ .l0 = 46.25 * 0.0254,
+ .l1 = 41.80 * 0.0254,
+ .m0 = 9.34 / 2.2,
+ .m1 = 9.77 / 2.2,
// Moment of inertia of the joints in kg m^2
- .j1 = 2957.05 * 0.0002932545454545454,
- .j2 = 2824.70 * 0.0002932545454545454,
+ .j0 = 2957.05 * 0.0002932545454545454,
+ .j1 = 2824.70 * 0.0002932545454545454,
// Radius of the center of mass of the joints in meters.
- .r1 = 21.64 * 0.0254,
- .r2 = 26.70 * 0.0254,
+ .r0 = 21.64 * 0.0254,
+ .r1 = 26.70 * 0.0254,
// Gear ratios for the two joints.
- .g1 = 140.0,
- .g2 = 90.0,
+ .g0 = 140.0,
+ .g1 = 90.0,
// MiniCIM motor constants.
.efficiency_tweak = kEfficiencyTweak,
diff --git a/frc971/control_loops/double_jointed_arm/trajectory.cc b/frc971/control_loops/double_jointed_arm/trajectory.cc
index d389eee..a67216b 100644
--- a/frc971/control_loops/double_jointed_arm/trajectory.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory.cc
@@ -105,7 +105,7 @@
::Eigen::Matrix<double, 2, 2> K2;
const ::Eigen::Matrix<double, 2, 1> gravity_volts =
- dynamics_->K3_inverse_ * dynamics_->GravityTorque(X);
+ dynamics_->K3_inverse() * dynamics_->GravityTorque(X);
dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
@@ -122,9 +122,9 @@
::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
const ::Eigen::Matrix<double, 2, 1> vk1 =
- dynamics_->K3_inverse_ * (K1 * alpha + K2 * omega_square * omega);
+ dynamics_->K3_inverse() * (K1 * alpha + K2 * omega_square * omega);
const ::Eigen::Matrix<double, 2, 1> vk2 =
- dynamics_->K3_inverse_ * dynamics_->K4_ * omega;
+ dynamics_->K3_inverse() * dynamics_->K4() * omega;
// Loop through all the various vmin, plan_vmax combinations.
for (const double c : {-plan_vmax, plan_vmax}) {
@@ -184,12 +184,12 @@
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- dynamics_->K3_inverse_ *
+ dynamics_->K3_inverse() *
((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
goal_velocity +
- dynamics_->K4_ * omega * goal_velocity - dynamics_->GravityTorque(X));
+ dynamics_->K4() * omega * goal_velocity - dynamics_->GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
- dynamics_->K3_inverse_ * K1 * omega;
+ dynamics_->K3_inverse() * K1 * omega;
const double constraint_goal_acceleration =
::std::sqrt(
@@ -242,12 +242,12 @@
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- dynamics_->K3_inverse_ *
+ dynamics_->K3_inverse() *
((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
goal_velocity +
- dynamics_->K4_ * omega * goal_velocity - dynamics_->GravityTorque(X));
+ dynamics_->K4() * omega * goal_velocity - dynamics_->GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
- dynamics_->K3_inverse_ * K1 * omega;
+ dynamics_->K3_inverse() * K1 * omega;
const double constraint_goal_acceleration =
::std::sqrt(
diff --git a/frc971/control_loops/fixed_quadrature.h b/frc971/control_loops/fixed_quadrature.h
index 572c235..22d4984 100644
--- a/frc971/control_loops/fixed_quadrature.h
+++ b/frc971/control_loops/fixed_quadrature.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
#define FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
+#include <Eigen/Dense>
#include <array>
namespace frc971 {
@@ -9,8 +10,8 @@
// Implements Gaussian Quadrature integration (5th order). fn is the function to
// integrate. It must take 1 argument of type T. The integration is between a
// and b.
-template <typename F, typename T>
-T GaussianQuadrature5(const F &fn, T a, T b) {
+template <typename T, typename F>
+double GaussianQuadrature5(const F &fn, T a, T b) {
// Pulled from Python.
// numpy.set_printoptions(precision=20)
// scipy.special.p_roots(5)
@@ -31,6 +32,30 @@
return answer;
}
+template <size_t N, typename F>
+Eigen::Matrix<double, N, 1> MatrixGaussianQuadrature5(const F &fn, double a,
+ double b) {
+ // Pulled from Python.
+ // numpy.set_printoptions(precision=20)
+ // scipy.special.p_roots(5)
+ const ::std::array<double, 5> x{{
+ -9.06179845938663630633e-01, -5.38469310105682885670e-01,
+ 3.24607628916367383789e-17, 5.38469310105683218737e-01,
+ 9.06179845938663408589e-01}};
+
+ const ::std::array<double, 5> w{{
+ 0.23692688505618844652, 0.4786286704993669705, 0.56888888888888811124,
+ 0.47862867049936674846, 0.23692688505618875183}};
+
+ Eigen::Matrix<double, N, 1> answer;
+ answer.setZero();
+ for (int i = 0; i < 5; ++i) {
+ const double y = (b - a) * (x[i] + 1) / 2.0 + a;
+ answer += (b - a) / 2.0 * w[i] * fn(y);
+ }
+ return answer;
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/fixed_quadrature_test.cc b/frc971/control_loops/fixed_quadrature_test.cc
index f842519..0615031 100644
--- a/frc971/control_loops/fixed_quadrature_test.cc
+++ b/frc971/control_loops/fixed_quadrature_test.cc
@@ -16,6 +16,18 @@
EXPECT_NEAR(y1, ::std::sin(0.5), 1e-15);
}
+// Tests that integrating y = [cos(x), sin(x)] works.
+TEST(GaussianQuadratureTest, MatrixCos) {
+ Eigen::Matrix<double, 2, 1> y1 = MatrixGaussianQuadrature5<2>(
+ [](double x) {
+ return Eigen::Matrix<double, 2, 1>(std::cos(x), std::sin(x));
+ },
+ 0.0, 0.5);
+
+ EXPECT_TRUE(y1.isApprox(Eigen::Matrix<double, 2, 1>(
+ ::std::sin(0.5), -std::cos(0.5) + std::cos(0))));
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 7b46439..c547833 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -29,6 +29,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -77,6 +78,7 @@
":controls",
":python_init",
"@pip//matplotlib",
+ "@pip//pygobject",
],
)
@@ -90,6 +92,7 @@
":controls",
":python_init",
"@pip//matplotlib",
+ "@pip//pygobject",
],
)
@@ -132,6 +135,7 @@
":python_init",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
],
)
@@ -153,6 +157,7 @@
"//aos/util:py_trapezoid_profile",
"//frc971/control_loops:python_init",
"@pip//matplotlib",
+ "@pip//pygobject",
],
)
@@ -166,6 +171,7 @@
"//aos/util:py_trapezoid_profile",
"//frc971/control_loops:python_init",
"@pip//matplotlib",
+ "@pip//pygobject",
],
)
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index 963f463..7fa3f3e 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -31,8 +31,8 @@
return X;
}
-// Implements Runge Kutta integration (4th order). This integrates dy/dt = fn(t,
-// y). It must have the call signature of fn(double t, T y). The
+// Implements Runge Kutta integration (4th order). This integrates dy/dt =
+// fn(t, y). It must have the call signature of fn(double t, T y). The
// integration starts at an initial value y, and integrates for dt.
template <typename F, typename T>
T RungeKutta(const F &fn, T y, double t, double dt) {
@@ -45,6 +45,15 @@
return y + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
}
+template <typename F, typename T>
+T RungeKuttaSteps(const F &fn, T X, double t, double dt, int steps) {
+ dt = dt / steps;
+ for (int i = 0; i < steps; ++i) {
+ X = RungeKutta(fn, X, t + dt * i, dt);
+ }
+ return X;
+}
+
// Implements Runge Kutta integration (4th order). fn is the function to
// integrate. It must take 1 argument of type T. The integration starts at an
// initial value X, and integrates for dt.
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
index 615f82c..e07c469 100644
--- a/frc971/control_loops/runge_kutta_test.cc
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -9,7 +9,7 @@
// Tests that integrating dx/dt = e^x works.
TEST(RungeKuttaTest, Exponential) {
::Eigen::Matrix<double, 1, 1> y0;
- y0(0, 0) = 0.0;
+ y0(0, 0) = 1.0;
::Eigen::Matrix<double, 1, 1> y1 = RungeKutta(
[](::Eigen::Matrix<double, 1, 1> x) {
@@ -18,7 +18,22 @@
return y;
},
y0, 0.1);
- EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
+ EXPECT_NEAR(y1(0, 0), -std::log(std::exp(-1.0) - 0.1), 1e-5);
+}
+
+// Now do it with sub steps.
+TEST(RungeKuttaTest, ExponentialSteps) {
+ ::Eigen::Matrix<double, 1, 1> y0;
+ y0(0, 0) = 1.0;
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKuttaSteps(
+ [](::Eigen::Matrix<double, 1, 1> x) {
+ ::Eigen::Matrix<double, 1, 1> y;
+ y(0, 0) = ::std::exp(x(0, 0));
+ return y;
+ },
+ y0, 0.1, 10);
+ EXPECT_NEAR(y1(0, 0), -std::log(std::exp(-1.0) - 0.1), 1e-8);
}
// Tests that integrating dx/dt = e^x works when we provide a U.
@@ -63,6 +78,20 @@
EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-3);
}
+// Now do it with a ton of sub steps.
+TEST(RungeKuttaTest, RungeKuttaTimeVaryingSteps) {
+ ::Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKuttaSteps(
+ [](double t, ::Eigen::Matrix<double, 1, 1> x) {
+ return (::Eigen::Matrix<double, 1, 1>()
+ << x(0, 0) * (2.0 / (::std::exp(t) + 1.0) - 1.0))
+ .finished();
+ },
+ y0, 5.0, 1.0, 10);
+ EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-7);
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/scouting/webserver/requests/messages/submit_actions.fbs b/scouting/webserver/requests/messages/submit_actions.fbs
index 9d9efa4..863e2fc 100644
--- a/scouting/webserver/requests/messages/submit_actions.fbs
+++ b/scouting/webserver/requests/messages/submit_actions.fbs
@@ -31,6 +31,8 @@
}
table EndMatchAction {
+ docked:bool (id:0);
+ engaged:bool (id:1);
}
union ActionType {
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
index 5704405..3bfba96 100644
--- a/y2014/control_loops/python/BUILD
+++ b/y2014/control_loops/python/BUILD
@@ -60,6 +60,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -76,6 +77,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -92,6 +94,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
diff --git a/y2016/control_loops/python/BUILD b/y2016/control_loops/python/BUILD
index ad3fbe9..6b54950 100644
--- a/y2016/control_loops/python/BUILD
+++ b/y2016/control_loops/python/BUILD
@@ -61,6 +61,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -94,6 +95,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -111,6 +113,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -140,6 +143,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -157,6 +161,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
diff --git a/y2017/control_loops/python/BUILD b/y2017/control_loops/python/BUILD
index 209bd70..a7b5624 100644
--- a/y2017/control_loops/python/BUILD
+++ b/y2017/control_loops/python/BUILD
@@ -58,6 +58,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -106,6 +107,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -123,6 +125,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
@@ -168,6 +171,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
diff --git a/y2018/constants.h b/y2018/constants.h
index 2a6f0ac..6adcb8a 100644
--- a/y2018/constants.h
+++ b/y2018/constants.h
@@ -51,7 +51,7 @@
}
static constexpr double kMaxProximalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g0 /
kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kProximalPotRatio() { return (12.0 / 60.0); }
@@ -60,7 +60,7 @@
static constexpr double kDistalEncoderRatio() { return (12.0 / 60.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 35f7433..fcedf40 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -94,6 +94,7 @@
"//frc971/control_loops/python:controls",
"@pip//glog",
"@pip//matplotlib",
+ "@pip//pygobject",
"@pip//python_gflags",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm_constants.h b/y2018/control_loops/superstructure/arm/arm_constants.h
index 9ac7020..1697a8e 100644
--- a/y2018/control_loops/superstructure/arm/arm_constants.h
+++ b/y2018/control_loops/superstructure/arm/arm_constants.h
@@ -14,22 +14,22 @@
constexpr double kStallCurrent = 89.0;
constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
- .l1 = 46.25 * 0.0254,
- .l2 = 41.80 * 0.0254,
- .m1 = 9.34 / 2.2,
- .m2 = 9.77 / 2.2,
+ .l0 = 46.25 * 0.0254,
+ .l1 = 41.80 * 0.0254,
+ .m0 = 9.34 / 2.2,
+ .m1 = 9.77 / 2.2,
// Moment of inertia of the joints in kg m^2
- .j1 = 2957.05 * 0.0002932545454545454,
- .j2 = 2824.70 * 0.0002932545454545454,
+ .j0 = 2957.05 * 0.0002932545454545454,
+ .j1 = 2824.70 * 0.0002932545454545454,
// Radius of the center of mass of the joints in meters.
- .r1 = 21.64 * 0.0254,
- .r2 = 26.70 * 0.0254,
+ .r0 = 21.64 * 0.0254,
+ .r1 = 26.70 * 0.0254,
// Gear ratios for the two joints.
- .g1 = 140.0,
- .g2 = 90.0,
+ .g0 = 140.0,
+ .g1 = 90.0,
// MiniCIM motor constants.
.efficiency_tweak = kEfficiencyTweak,
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index 8a1f737..3833536 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -122,6 +122,7 @@
deps = [
"//frc971/control_loops/python:controls",
"@pip//matplotlib",
+ "@pip//pygobject",
],
)
diff --git a/y2023/BUILD b/y2023/BUILD
index ad82c2b..40aa841 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -291,6 +291,7 @@
"//y2023/control_loops/drivetrain:drivetrain_base",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//y2023/control_loops/superstructure/arm:generated_graph",
],
)
diff --git a/y2023/constants.h b/y2023/constants.h
index f8e766c..0475242 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -60,7 +60,7 @@
static constexpr double kProximalEncoderRatio() { return (15.0 / 95.0); }
static constexpr double kMaxProximalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g0 /
kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kProximalPotRatio() {
@@ -76,8 +76,8 @@
static constexpr double kDistalEncoderRatio() { return (15.0 / 95.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
- kDistalEncoderRatio() * kDistalEncoderCountsPerRevolution();
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
return (24.0 / 36.0) * (18.0 / 66.0) * (15.0 / 95.0);
@@ -139,6 +139,10 @@
};
}
+ // Rollers
+ static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
+ static constexpr double kRollerStatorCurrentLimit() { return 60.0; }
+
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::RelativeEncoderZeroingEstimator>
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 718a575..f5eff79 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -68,7 +68,6 @@
"superstructure.h",
],
deps = [
- # ":collision_avoidance_lib",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -78,6 +77,7 @@
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2023:constants",
+ "//y2023/control_loops/superstructure/arm",
],
)
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index dc52f02..c125fac 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -62,3 +62,19 @@
"//frc971/control_loops/double_jointed_arm:dynamics",
],
)
+
+cc_binary(
+ name = "arm_design",
+ srcs = [
+ "arm_design.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":arm_constants",
+ "//aos:init",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:jacobian",
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ ],
+)
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index a595ace..27588f1 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -26,18 +26,16 @@
// if true, tune down all the constants for testing.
static constexpr bool kGrannyMode() { return false; }
+
// the operating voltage.
static constexpr double kOperatingVoltage() {
return kGrannyMode() ? 5.0 : 12.0;
}
-
static constexpr double kDt() { return 0.00505; }
-
static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
-
static constexpr double kPathlessVMax() { return 5.0; }
static constexpr double kGotoPathVMax() { return 6.0; }
@@ -50,16 +48,19 @@
void Reset();
ArmState state() const { return state_; }
- bool estopped() const { return state_ == ArmState::ESTOP; }
+ bool estopped() const { return state_ == ArmState::ESTOP; }
bool zeroed() const {
return (proximal_zeroing_estimator_.zeroed() &&
distal_zeroing_estimator_.zeroed());
}
+
// Returns the maximum position for the intake. This is used to override the
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }
+
uint32_t current_node() const { return current_node_; }
+
double path_distance_to_go() { return follower_.path_distance_to_go(); }
private:
@@ -71,6 +72,7 @@
}
std::shared_ptr<const constants::Values> values_;
+
ArmState state_;
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
@@ -80,15 +82,16 @@
double proximal_offset_;
double distal_offset_;
+
double max_intake_override_;
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+
double vmax_ = kVMax();
frc971::control_loops::arm::Dynamics dynamics_;
::std::vector<TrajectoryAndParams> trajectories_;
-
SearchGraph search_graph_;
bool close_enough_for_full_power_;
@@ -96,7 +99,6 @@
size_t brownout_count_;
EKF arm_ekf_;
-
TrajectoryFollower follower_;
const ::std::vector<::Eigen::Matrix<double, 2, 1>> points_;
@@ -106,6 +108,7 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
+
} // namespace arm
} // namespace superstructure
} // namespace control_loops
diff --git a/y2023/control_loops/superstructure/arm/arm_constants.h b/y2023/control_loops/superstructure/arm/arm_constants.h
index 5611eaa..39e146f 100644
--- a/y2023/control_loops/superstructure/arm/arm_constants.h
+++ b/y2023/control_loops/superstructure/arm/arm_constants.h
@@ -14,23 +14,23 @@
constexpr double kStallCurrent = 257.0;
constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
- .l1 = 20 * 0.0254,
- .l2 = 38 * 0.0254,
+ .l0 = 20 * 0.0254,
+ .l1 = 38 * 0.0254,
- .m1 = 9.34 / 2.2,
- .m2 = 9.77 / 2.2,
+ .m0 = 9.34 / 2.2,
+ .m1 = 9.77 / 2.2,
// Moment of inertia of the joints in kg m^2
- .j1 = 2957.05 * 0.0002932545454545454,
- .j2 = 2824.70 * 0.0002932545454545454,
+ .j0 = 2957.05 * 0.0002932545454545454,
+ .j1 = 2824.70 * 0.0002932545454545454,
// Radius of the center of mass of the joints in meters.
- .r1 = 21.64 * 0.0254,
- .r2 = 26.70 * 0.0254,
+ .r0 = 21.64 * 0.0254,
+ .r1 = 26.70 * 0.0254,
// Gear ratios for the two joints.
- .g1 = 55.0,
- .g2 = 106.0,
+ .g0 = 55.0,
+ .g1 = 106.0,
// Falcon motor constants.
.efficiency_tweak = kEfficiencyTweak,
diff --git a/y2023/control_loops/superstructure/arm/arm_design.cc b/y2023/control_loops/superstructure/arm/arm_design.cc
new file mode 100644
index 0000000..99ffa2c
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm_design.cc
@@ -0,0 +1,193 @@
+#include "aos/init.h"
+#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/jacobian.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+
+DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
+DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
+DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
+DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
+DEFINE_double(fx, 0.0, "X force");
+DEFINE_double(fy, 0.0, "y force");
+
+DEFINE_double(start0, 0.0, "starting position on proximal");
+DEFINE_double(start1, 0.0, "starting position on distal");
+DEFINE_double(goal0, 0.0, "goal position on proximal");
+DEFINE_double(goal1, 0.0, "goal position on distal");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+int Main() {
+ frc971::analysis::Plotter plotter;
+
+ frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+
+ ::Eigen::Matrix<double, 4, 1> X = ::Eigen::Matrix<double, 4, 1>::Zero();
+ ::Eigen::Matrix<double, 4, 1> goal = ::Eigen::Matrix<double, 4, 1>::Zero();
+ goal(0, 0) = FLAGS_goal0;
+ goal(1, 0) = 0;
+ goal(2, 0) = FLAGS_goal1;
+ goal(3, 0) = 0;
+
+ X(0, 0) = FLAGS_start0;
+ X(1, 0) = 0;
+ X(2, 0) = FLAGS_start1;
+ X(3, 0) = 0;
+ ::Eigen::Matrix<double, 2, 1> U = ::Eigen::Matrix<double, 2, 1>::Zero();
+
+ constexpr double kDt = 0.00505;
+
+ std::vector<double> t;
+ std::vector<double> x0;
+ std::vector<double> x1;
+ std::vector<double> x2;
+ std::vector<double> x3;
+ std::vector<double> u0;
+ std::vector<double> u1;
+
+ std::vector<double> current0;
+ std::vector<double> current1;
+ std::vector<double> torque0;
+ std::vector<double> torque1;
+
+ const double kProximalPosLQR = FLAGS_lqr_proximal_pos;
+ const double kProximalVelLQR = FLAGS_lqr_proximal_vel;
+ const double kDistalPosLQR = FLAGS_lqr_distal_pos;
+ const double kDistalVelLQR = FLAGS_lqr_distal_vel;
+ const ::Eigen::DiagonalMatrix<double, 4> Q =
+ (::Eigen::DiagonalMatrix<double, 4>().diagonal()
+ << 1.0 / ::std::pow(kProximalPosLQR, 2),
+ 1.0 / ::std::pow(kProximalVelLQR, 2), 1.0 / ::std::pow(kDistalPosLQR, 2),
+ 1.0 / ::std::pow(kDistalVelLQR, 2))
+ .finished()
+ .asDiagonal();
+
+ const ::Eigen::DiagonalMatrix<double, 2> R =
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal();
+
+ {
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ dynamics
+ .TorqueFromForce(X,
+ ::Eigen::Matrix<double, 2, 1>(FLAGS_fx, FLAGS_fy))
+ .transpose();
+ LOG(INFO) << "Torque (N m): " << torque.transpose();
+ const ::Eigen::Matrix<double, 2, 1> current =
+ dynamics.CurrentFromTorque(torque);
+
+ LOG(INFO) << "Current (Amps): " << current.transpose();
+
+ ::Eigen::Matrix<double, 2, 1> battery_current;
+ battery_current(0) =
+ current(0) * current(0) * kArmConstants.resistance / 12.0;
+ battery_current(1) =
+ current(1) * current(1) * kArmConstants.resistance / 12.0;
+
+ LOG(INFO) << "Battery current (Amps): " << battery_current.transpose();
+ }
+
+ ::Eigen::Matrix<double, 2, 4> last_K = ::Eigen::Matrix<double, 2, 4>::Zero();
+ for (int i = 0; i < 400; ++i) {
+ t.push_back(i * kDt);
+ x0.push_back(X(0));
+ x1.push_back(X(1));
+ x2.push_back(X(2));
+ x3.push_back(X(3));
+
+ const auto x_blocked = X.block<4, 1>(0, 0);
+
+ const ::Eigen::Matrix<double, 4, 4> final_A =
+ ::frc971::control_loops::NumericalJacobianX<4, 2>(
+ [dynamics](const auto &x_blocked, const auto &U, double kDt) {
+ return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
+ },
+ x_blocked, U, 0.00505);
+ const ::Eigen::Matrix<double, 4, 2> final_B =
+ ::frc971::control_loops::NumericalJacobianU<4, 2>(
+ [dynamics](const auto &x_blocked, const auto &U, double kDt) {
+ return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
+ },
+ x_blocked, U, 0.00505);
+
+ ::Eigen::Matrix<double, 2, 1> U_ff =
+ dynamics.FF_U(x_blocked, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
+
+ ::Eigen::Matrix<double, 4, 4> S;
+ ::Eigen::Matrix<double, 2, 4> K;
+ if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &K, &S) == 0) {
+ ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
+ final_A - final_B * K);
+
+ last_K = K;
+ } else {
+ LOG(INFO) << "Failed to solve for K at " << i;
+ }
+ U = U_ff + last_K * (goal - X);
+ if (std::abs(U(0)) > 12.0) {
+ U /= std::abs(U(0)) / 12.0;
+ }
+ if (std::abs(U(1)) > 12.0) {
+ U /= std::abs(U(1)) / 12.0;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ dynamics.TorqueFromCommand(X, U);
+ const ::Eigen::Matrix<double, 2, 1> current_per_motor =
+ dynamics.CurrentFromCommand(X, U);
+
+ current0.push_back(current_per_motor(0));
+ current1.push_back(current_per_motor(1));
+ torque0.push_back(torque(0));
+ torque1.push_back(torque(1));
+
+ u0.push_back(U(0));
+ u1.push_back(U(1));
+
+ X = dynamics.UnboundedDiscreteDynamics(X, U, kDt);
+ }
+
+ plotter.Title("Arm motion");
+ plotter.AddFigure("State");
+ plotter.AddLine(t, x0, "X 0");
+ plotter.AddLine(t, x1, "X 1");
+ plotter.AddLine(t, x2, "X 2");
+ plotter.AddLine(t, x3, "X 3");
+
+ plotter.AddLine(t, u0, "U 0");
+ plotter.AddLine(t, u1, "U 1");
+ plotter.Publish();
+
+ plotter.AddFigure("Command");
+ plotter.AddLine(t, u0, "U 0");
+ plotter.AddLine(t, u1, "U 1");
+
+ plotter.AddLine(t, current0, "current 0");
+ plotter.AddLine(t, current1, "current 1");
+ plotter.AddLine(t, torque0, "torque 0");
+ plotter.AddLine(t, torque1, "torque 1");
+ plotter.Publish();
+
+ plotter.Spin();
+
+ return 0;
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+ return y2023::control_loops::superstructure::arm::Main();
+}
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 1125e21..fa83d20 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -12,6 +12,8 @@
namespace control_loops {
namespace superstructure {
+using ::aos::monotonic_clock;
+
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::RelativeEncoderProfiledJointStatus;
@@ -26,19 +28,19 @@
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
- (void)values;
-}
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ arm_(values_) {}
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
- (void)unsafe_goal;
- (void)position;
+ const monotonic_clock::time_point timestamp =
+ event_loop()->context().monotonic_event_time;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ arm_.Reset();
}
OutputT output_struct;
@@ -47,11 +49,31 @@
alliance_ = joystick_state_fetcher_->alliance();
}
drivetrain_status_fetcher_.Fetch();
- output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
-
+
+ const uint32_t arm_goal_position =
+ unsafe_goal != nullptr ? unsafe_goal->arm_goal_position() : 0u;
+
+ flatbuffers::Offset<superstructure::ArmStatus> arm_status_offset =
+ arm_.Iterate(
+ timestamp, unsafe_goal != nullptr ? &(arm_goal_position) : nullptr,
+ position->arm(),
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
+ output != nullptr ? &output_struct.proximal_voltage : nullptr,
+ output != nullptr ? &output_struct.distal_voltage : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->intake() : false,
+ unsafe_goal != nullptr ? unsafe_goal->spit() : false,
+
+ status->fbb());
+
+ if (output) {
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+ }
+
Status::Builder status_builder = status->MakeBuilder<Status>();
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
+ status_builder.add_arm(arm_status_offset);
+
(void)status->Send(status_builder.Finish());
}
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index 0740de3..2d0fc43 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -5,6 +5,7 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023/constants.h"
+#include "y2023/control_loops/superstructure/arm/arm.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
@@ -45,6 +46,8 @@
drivetrain_status_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ arm::Arm arm_;
+
aos::Alliance alliance_ = aos::Alliance::kInvalid;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 47ce7b2..936cbee 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -8,13 +8,16 @@
// Controls distal, proximal, and roll joints
arm_goal_position:uint32 (id: 0);
- wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 1);
+ // Overrides the current path to go to the next path
+ trajectory_override:bool (id: 1);
+
+ wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
// If this is true, the rollers should intake.
- intake:bool (id: 2);
+ intake:bool (id: 3);
// If this is true, the rollers should spit.
- spit:bool (id: 3);
+ spit:bool (id: 4);
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 0b9fb7d..ae01f0b 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -18,6 +18,9 @@
namespace control_loops {
namespace superstructure {
namespace testing {
+namespace {
+constexpr double kNoiseScalar = 0.01;
+} // namespace
namespace chrono = std::chrono;
using ::aos::monotonic_clock;
@@ -40,6 +43,89 @@
frc971::control_loops::RelativeEncoderProfiledJointStatus,
RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+class ArmSimulation {
+ public:
+ explicit ArmSimulation(
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &proximal_zeroing_constants,
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &distal_zeroing_constants,
+ std::chrono::nanoseconds dt)
+ : proximal_zeroing_constants_(proximal_zeroing_constants),
+ proximal_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kProximalEncoderRatio()),
+ distal_zeroing_constants_(distal_zeroing_constants),
+ distal_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kDistalEncoderRatio()),
+ dynamics_(arm::kArmConstants),
+ dt_(dt) {
+ X_.setZero();
+ }
+
+ void InitializePosition(::Eigen::Matrix<double, 2, 1> position) {
+ X_ << position(0), 0.0, position(1), 0.0;
+
+ proximal_pot_encoder_.Initialize(
+ X_(0), kNoiseScalar, 0.0,
+ proximal_zeroing_constants_.measured_absolute_position);
+ distal_pot_encoder_.Initialize(
+ X_(2), kNoiseScalar, 0.0,
+ distal_zeroing_constants_.measured_absolute_position);
+ }
+
+ flatbuffers::Offset<ArmPosition> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder proximal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
+ proximal_pot_encoder_.GetSensorValues(&proximal_builder);
+
+ frc971::PotAndAbsolutePosition::Builder distal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
+ distal_pot_encoder_.GetSensorValues(&distal_builder);
+
+ ArmPosition::Builder arm_position_builder(*fbb);
+ arm_position_builder.add_proximal(proximal_offset);
+ arm_position_builder.add_distal(distal_offset);
+
+ return arm_position_builder.Finish();
+ }
+
+ double proximal_position() const { return X_(0, 0); }
+ double proximal_velocity() const { return X_(1, 0); }
+ double distal_position() const { return X_(2, 0); }
+ double distal_velocity() const { return X_(3, 0); }
+
+ void Simulate(::Eigen::Matrix<double, 2, 1> U) {
+ constexpr double voltage_check =
+ superstructure::arm::Arm::kOperatingVoltage();
+
+ AOS_CHECK_LE(::std::abs(U(0)), voltage_check);
+ AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
+
+ X_ = dynamics_.UnboundedDiscreteDynamics(
+ X_, U,
+ std::chrono::duration_cast<std::chrono::duration<double>>(dt_).count());
+
+ // TODO(austin): Estop on grose out of bounds.
+ proximal_pot_encoder_.MoveTo(X_(0));
+ distal_pot_encoder_.MoveTo(X_(2));
+ }
+
+ private:
+ ::Eigen::Matrix<double, 4, 1> X_;
+
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ proximal_zeroing_constants_;
+ PositionSensorSimulator proximal_pot_encoder_;
+
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ distal_zeroing_constants_;
+ PositionSensorSimulator distal_pot_encoder_;
+
+ ::frc971::control_loops::arm::Dynamics dynamics_;
+
+ std::chrono::nanoseconds dt_;
+};
// Class which simulates the superstructure and sends out queue messages with
// the position.
class SuperstructureSimulation {
@@ -49,21 +135,26 @@
chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
+ arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing, dt_),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")) {
- (void)values;
- (void)dt_;
-
+ InitializeArmPosition(arm::NeutralPosPoint());
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
if (!first_) {
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ arm_.Simulate(
+ (::Eigen::Matrix<double, 2, 1>()
+ << superstructure_output_fetcher_->proximal_voltage(),
+ superstructure_output_fetcher_->distal_voltage())
+ .finished());
}
first_ = false;
SendPositionMessage();
@@ -71,13 +162,22 @@
dt);
}
+ void InitializeArmPosition(::Eigen::Matrix<double, 2, 1> position) {
+ arm_.InitializePosition(position);
+ }
+
+ const ArmSimulation &arm() const { return arm_; }
+
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
- Position::Builder position_builder = builder.MakeBuilder<Position>();
+ flatbuffers::Offset<ArmPosition> arm_offset =
+ arm_.GetSensorValues(builder.fbb());
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_arm(arm_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -87,6 +187,8 @@
const chrono::nanoseconds dt_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+ ArmSimulation arm_;
+
::aos::Sender<Position> superstructure_position_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
@@ -139,6 +241,10 @@
void VerifyNearGoal() {
superstructure_goal_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
+
+ ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
+ << ": No status";
}
void CheckIfZeroed() {
@@ -233,6 +339,8 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -289,6 +397,73 @@
CheckIfZeroed();
}
+// Tests that we don't freak out without a goal.
+TEST_F(SuperstructureTest, ArmSimpleGoal) {
+ SetEnabled(true);
+ RunFor(chrono::seconds(20));
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralToConePos1Index());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(ArmState::RUNNING, superstructure_status_fetcher_->arm()->state());
+}
+
+// Tests that we can can execute a move.
+TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
+ SetEnabled(true);
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralToConePos1Index());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralToConePos2Index());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+// Tests that we can can execute a move which moves through multiple nodes.
+TEST_F(SuperstructureTest, ArmMultistepMove) {
+ SetEnabled(true);
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::ConePosIndex());
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 1c8c0e9..8549943 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -5,10 +5,12 @@
table ArmPosition {
// Values of the encoder and potentiometer at the base of the proximal
// (connected to drivebase) arm in radians.
+ // Zero is upwards, positive is a forwards rotation
proximal:frc971.PotAndAbsolutePosition (id: 0);
// Values of the encoder and potentiometer at the base of the distal
// (connected to proximal) arm in radians.
+ // Zero is straight up, positive is a forwards rotation
distal:frc971.PotAndAbsolutePosition (id: 1);
}
@@ -21,7 +23,7 @@
// Zero for wrist is facing staright outward.
// Positive position would be upwards
- wrist:frc971.PotAndAbsolutePosition (id: 2);
+ wrist:frc971.AbsolutePosition (id: 2);
}
root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index b2cad0c..9b21cc3 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -62,7 +62,7 @@
roll_joint:frc971.PotAndAbsoluteEncoderEstimatorState (id: 3);
- wrist:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
+ wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 4);
}
root_type Status;
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index a1a42f4..3d019f1 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -19,6 +19,7 @@
#include "frc971/zeroing/wrap.h"
#include "y2023/constants.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+#include "y2023/control_loops/superstructure/arm/generated_graph.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
@@ -34,7 +35,13 @@
namespace input {
namespace joysticks {
+// TODO(milind): add correct locations
+const ButtonLocation kIntake(3, 3);
+const ButtonLocation kScore(3, 3);
+const ButtonLocation kSpit(3, 3);
+
namespace superstructure = y2023::control_loops::superstructure;
+namespace arm = superstructure::arm;
class Reader : public ::frc971::input::ActionJoystickInput {
public:
@@ -59,14 +66,28 @@
return;
}
- (void)data;
- // TODO(Xander): Use driverstaion data to provide instructions.
+ bool intake = false;
+ bool spit = false;
+
+ // TODO(milind): add more actions and paths
+ if (data.IsPressed(kIntake)) {
+ intake = true;
+ arm_goal_position_ = arm::ConePosIndex();
+ } else if (data.IsPressed(kSpit)) {
+ spit = true;
+ arm_goal_position_ = arm::ConePosIndex();
+ } else if (data.IsPressed(kScore)) {
+ arm_goal_position_ = arm::ConePerchPosIndex();
+ }
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
-
+ superstructure_goal_builder.add_arm_goal_position(arm_goal_position_);
+ superstructure_goal_builder.add_intake(intake);
+ superstructure_goal_builder.add_spit(spit);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
@@ -78,6 +99,8 @@
::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+
+ uint32_t arm_goal_position_;
};
} // namespace joysticks
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 690d4bf..da2bebb 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -86,12 +86,27 @@
control_loops::drivetrain::kWheelRadius;
}
-constexpr double kMaxFastEncoderPulsesPerSecond =
- /*std::max({*/ Values::kMaxDrivetrainEncoderPulsesPerSecond() /*,
- Values::kMaxIntakeEncoderPulsesPerSecond()})*/
- ;
-/*static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
- "fast encoders are too fast");*/
+double proximal_pot_translate(double voltage) {
+ return voltage * Values::kProximalPotRadiansPerVolt();
+}
+
+double distal_pot_translate(double voltage) {
+ return voltage * Values::kDistalPotRadiansPerVolt();
+}
+
+double roll_joint_pot_translate(double voltage) {
+ return voltage * Values::kRollJointPotRadiansPerVolt();
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+ Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxProximalEncoderPulsesPerSecond(),
+ Values::kMaxDistalEncoderPulsesPerSecond(),
+ Values::kMaxRollJointEncoderPulsesPerSecond(),
+ Values::kMaxWristEncoderPulsesPerSecond(),
+});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
} // namespace
@@ -143,7 +158,48 @@
void RunIteration() override {
superstructure_reading_->Set(true);
- { auto builder = superstructure_position_sender_.MakeBuilder(); }
+ {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ frc971::PotAndAbsolutePositionT proximal;
+ CopyPosition(proximal_encoder_, &proximal,
+ Values::kProximalEncoderCountsPerRevolution(),
+ Values::kProximalEncoderRatio(), proximal_pot_translate,
+ false, values_->arm_proximal.potentiometer_offset);
+ frc971::PotAndAbsolutePositionT distal;
+ CopyPosition(distal_encoder_, &distal,
+ Values::kDistalEncoderCountsPerRevolution(),
+ Values::kDistalEncoderRatio(), distal_pot_translate, false,
+ values_->arm_distal.potentiometer_offset);
+ frc971::PotAndAbsolutePositionT roll_joint;
+ CopyPosition(roll_joint_encoder_, &roll_joint,
+ Values::kRollJointEncoderCountsPerRevolution(),
+ Values::kRollJointEncoderRatio(), roll_joint_pot_translate,
+ false, values_->roll_joint.potentiometer_offset);
+ frc971::AbsolutePositionT wrist;
+ CopyPosition(wrist_encoder_, &wrist,
+ Values::kWristEncoderCountsPerRevolution(),
+ Values::kWristEncoderRatio(), false);
+
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &proximal);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &distal);
+ flatbuffers::Offset<superstructure::ArmPosition> arm_offset =
+ superstructure::CreateArmPosition(*builder.fbb(), proximal_offset,
+ distal_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> roll_joint_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &roll_joint);
+ flatbuffers::Offset<frc971::AbsolutePosition> wrist_offset =
+ frc971::AbsolutePosition::Pack(*builder.fbb(), &wrist);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_arm(arm_offset);
+ position_builder.add_roll_joint(roll_joint_offset);
+ position_builder.add_wrist(wrist_offset);
+ builder.CheckOk(builder.Send(position_builder.Finish()));
+ }
{
auto builder = drivetrain_position_sender_.MakeBuilder();
@@ -228,6 +284,61 @@
superstructure_reading_ = superstructure_reading;
}
+ void set_proximal_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ proximal_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_proximal_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ proximal_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_proximal_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ proximal_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_distal_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ distal_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_distal_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ distal_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_distal_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ distal_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_roll_joint_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ roll_joint_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_roll_joint_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ roll_joint_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_roll_joint_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ roll_joint_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_wrist_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ wrist_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_wrist_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
private:
std::shared_ptr<const Values> values_;
@@ -242,6 +353,10 @@
std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_;
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
+
+ frc971::wpilib::AbsoluteEncoderAndPotentiometer proximal_encoder_,
+ distal_encoder_, roll_joint_encoder_;
+ frc971::wpilib::AbsoluteEncoder wrist_encoder_;
};
class SuperstructureWriter
@@ -258,10 +373,50 @@
superstructure_reading_ = superstructure_reading;
}
- private:
- void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+ void set_proximal_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ proximal_falcon_ = ::std::move(t);
+ }
- void Write(const superstructure::Output &output) override { (void)output; }
+ void set_distal_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ distal_falcon_ = ::std::move(t);
+ }
+
+ void set_roll_joint_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ roll_joint_victor_ = ::std::move(t);
+ }
+
+ void set_wrist_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ wrist_victor_ = ::std::move(t);
+ }
+
+ void set_roller_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ roller_falcon_ = ::std::move(t);
+ roller_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kRollerSupplyCurrentLimit(),
+ Values::kRollerSupplyCurrentLimit(), 0});
+ roller_falcon_->ConfigStatorCurrentLimit(
+ {true, Values::kRollerStatorCurrentLimit(),
+ Values::kRollerStatorCurrentLimit(), 0});
+ }
+
+ private:
+ void Stop() override {
+ AOS_LOG(WARNING, "Superstructure output too old.\n");
+ proximal_falcon_->SetDisabled();
+ distal_falcon_->SetDisabled();
+ roll_joint_victor_->SetDisabled();
+ wrist_victor_->SetDisabled();
+ roller_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ }
+
+ void Write(const superstructure::Output &output) override {
+ WritePwm(output.proximal_voltage(), proximal_falcon_.get());
+ WritePwm(output.distal_voltage(), distal_falcon_.get());
+ WritePwm(output.roll_joint_voltage(), roll_joint_victor_.get());
+ WritePwm(output.wrist_voltage(), wrist_victor_.get());
+ WriteCan(output.roller_voltage(), roller_falcon_.get());
+ }
static void WriteCan(const double voltage,
::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
@@ -275,10 +430,14 @@
motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
12.0);
}
+
+ ::std::unique_ptr<::frc::TalonFX> proximal_falcon_, distal_falcon_;
+ ::std::unique_ptr<::frc::VictorSP> roll_joint_victor_, wrist_victor_;
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> roller_falcon_;
};
static constexpr int kCANFalconCount = 6;
-static constexpr int kCANSignalsCount = 3;
+static constexpr int kCANSignalsCount = 4;
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
class Falcon {
@@ -296,9 +455,9 @@
// device temp is not timesynced so don't add it to the list of signals
device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
- CHECK_EQ(kCANSignalsCount, 3);
+ CHECK_EQ(kCANSignalsCount, 4);
CHECK_NOTNULL(signals);
- CHECK_LE(signals->size() + 3u, signals->capacity());
+ CHECK_LE(signals->size() + 4u, signals->capacity());
supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
signals->push_back(&supply_voltage_);
@@ -429,14 +588,15 @@
private:
void Loop() {
- CHECK_EQ(signals_.size(), 18u);
+ CHECK_EQ(signals_.size(), 24u);
ctre::phoenix::StatusCode status =
ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(
- 2000_ms,
- {signals_[0], signals_[1], signals_[2], signals_[3], signals_[4],
- signals_[5], signals_[6], signals_[7], signals_[8], signals_[9],
- signals_[10], signals_[11], signals_[12], signals_[13],
- signals_[14], signals_[15], signals_[16], signals_[17]});
+ 2000_ms, {signals_[0], signals_[1], signals_[2], signals_[3],
+ signals_[4], signals_[5], signals_[6], signals_[7],
+ signals_[8], signals_[9], signals_[10], signals_[11],
+ signals_[12], signals_[13], signals_[14], signals_[15],
+ signals_[16], signals_[17], signals_[18], signals_[19],
+ signals_[20], signals_[21], signals_[22], signals_[23]});
if (!status.IsOK()) {
AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
@@ -633,9 +793,25 @@
sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_superstructure_reading(superstructure_reading);
+ sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(1));
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
- sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(9));
- sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(8));
+ sensor_reader.set_proximal_encoder(make_encoder(3));
+ sensor_reader.set_proximal_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_proximal_potentiometer(make_unique<frc::AnalogInput>(3));
+
+ sensor_reader.set_distal_encoder(make_encoder(2));
+ sensor_reader.set_distal_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_distal_potentiometer(make_unique<frc::AnalogInput>(2));
+
+ sensor_reader.set_roll_joint_encoder(make_encoder(5));
+ sensor_reader.set_roll_joint_absolute_pwm(
+ make_unique<frc::DigitalInput>(5));
+ sensor_reader.set_roll_joint_potentiometer(
+ make_unique<frc::AnalogInput>(5));
+
+ sensor_reader.set_wrist_encoder(make_encoder(4));
+ sensor_reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(4));
AddLoop(&sensor_reader_event_loop);
@@ -671,9 +847,18 @@
ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive);
drivetrain_writer.set_left_inverted(
ctre::phoenixpro::signals::InvertedValue::CounterClockwise_Positive);
-
SuperstructureWriter superstructure_writer(&output_event_loop);
+ superstructure_writer.set_proximal_falcon(make_unique<::frc::TalonFX>(1));
+ superstructure_writer.set_distal_falcon(make_unique<::frc::TalonFX>(0));
+
+ superstructure_writer.set_roll_joint_victor(
+ make_unique<::frc::VictorSP>(3));
+ superstructure_writer.set_wrist_victor(make_unique<::frc::VictorSP>(2));
+
+ superstructure_writer.set_roller_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+
superstructure_writer.set_superstructure_reading(superstructure_reading);
AddLoop(&output_event_loop);
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index e12d612..e4a9c38 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -61,7 +61,7 @@
"source_node": "roborio",
"frequency": 50,
"num_senders": 20,
- "max_size": 4096
+ "max_size": 8192
},
{
"name": "/roborio/aos",
@@ -530,7 +530,7 @@
]
},
{
- "name": "constants_sender",
+ "name": "constants_sender_roborio",
"autorestart": false,
"nodes": [
"roborio"