Merge "Only update the catapult firing bool when enabled"
diff --git a/frc971/control_loops/capped_test_plant.h b/frc971/control_loops/capped_test_plant.h
index 65bc7cc..f4286cd 100644
--- a/frc971/control_loops/capped_test_plant.h
+++ b/frc971/control_loops/capped_test_plant.h
@@ -29,4 +29,4 @@
} // namespace frc971
} // namespace control_loops
-#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
\ No newline at end of file
+#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index fd39359..46e6a05 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -39,7 +39,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
- const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min, bool delayed_u)
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min, size_t delayed_u)
: A_continuous(A_continuous),
B_continuous(B_continuous),
C(C),
@@ -55,7 +55,7 @@
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
- const bool delayed_u;
+ const size_t delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -230,7 +230,7 @@
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
P_steady_state;
- const bool delayed_u;
+ const size_t delayed_u;
HybridKalmanCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
@@ -238,7 +238,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
&R_continuous,
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
- &P_steady_state, bool delayed_u)
+ &P_steady_state, size_t delayed_u)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
P_steady_state(P_steady_state), delayed_u(delayed_u) {
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index f431983..1649dd2 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -266,7 +266,7 @@
name: string, The name of the loop to use when writing the C++ files.
"""
self._name = name
- self.delayed_u = False
+ self.delayed_u = 0
@property
def name(self):
@@ -291,7 +291,7 @@
self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
- self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], 1)))
+ self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -314,19 +314,21 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
#U = numpy.clip(U, self.U_min, self.U_max)
- if self.delayed_u:
- self.X = self.A * self.X + self.B * self.last_U
- self.Y = self.C * self.X + self.D * self.last_U
- self.last_U = U.copy()
+ if self.delayed_u > 0:
+ self.X = self.A * self.X + self.B * self.last_U[:, -1]
+ self.Y = self.C * self.X + self.D * self.last_U[:, -1]
+ self.last_U[:, 1:] = self.last_U[:, 0:-1]
+ self.last_U[:, 0] = U.copy()
else:
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U
def PredictObserver(self, U):
"""Runs the predict step of the observer update."""
- if self.delayed_u:
- self.X_hat = (self.A * self.X_hat + self.B * self.last_U)
- self.last_U = U.copy()
+ if self.delayed_u > 0:
+ self.X_hat = (self.A * self.X_hat + self.B * self.last_U[:, -1])
+ self.last_U[:, 1:] = self.last_U[:, 0:-1]
+ self.last_U[:, 0] = U.copy()
else:
self.X_hat = (self.A * self.X_hat + self.B * U)
@@ -336,9 +338,9 @@
KalmanGain = self.KalmanGain
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
- if self.delayed_u:
+ if self.delayed_u > 0:
self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
- self.D * self.last_U)
+ self.D * self.last_U[:, -1])
else:
self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
self.D * U)
@@ -396,7 +398,7 @@
ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
- delayed_u_string = "true" if self.delayed_u else "false"
+ delayed_u_string = str(self.delayed_u)
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
@@ -492,7 +494,7 @@
'%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
]
- delayed_u_string = "true" if self.delayed_u else "false"
+ delayed_u_string = str(self.delayed_u)
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
@@ -540,9 +542,10 @@
def PredictHybridObserver(self, U, dt):
self.Discretize(dt)
- if self.delayed_u:
- self.X_hat = self.A * self.X_hat + self.B * self.last_U
- self.last_U = U.copy()
+ if self.delayed_u > 0:
+ self.X_hat = self.A * self.X_hat + self.B * self.last_U[:, -1]
+ self.last_U[:, 1:] = self.last_U[:, 0:-1]
+ self.last_U[:, 0] = U.copy()
else:
self.X_hat = self.A * self.X_hat + self.B * U
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 53cd6a2..1ea94d9 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -12,6 +12,7 @@
#if defined(__linux__)
#include "aos/logging/logging.h"
+#include "glog/logging.h"
#endif
#include "aos/macros.h"
@@ -49,7 +50,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min,
- const std::chrono::nanoseconds dt, bool delayed_u)
+ const std::chrono::nanoseconds dt, size_t delayed_u)
: A(A),
B(B),
C(C),
@@ -71,7 +72,7 @@
// useful for modeling a control loop cycle where you sample, compute, and
// then queue the outputs to be ready to be executed when the next cycle
// happens.
- const bool delayed_u;
+ const size_t delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -85,6 +86,16 @@
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
&&coefficients)
: coefficients_(::std::move(coefficients)), index_(0) {
+ if (coefficients_.size() > 1u) {
+ for (size_t i = 1; i < coefficients_.size(); ++i) {
+ if (coefficients_[i]->delayed_u != coefficients_[0]->delayed_u) {
+ abort();
+ }
+ }
+ }
+ last_U_ = Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic>(
+ number_of_inputs,
+ std::max(static_cast<size_t>(1u), coefficients_[0]->delayed_u));
Reset();
}
@@ -175,15 +186,27 @@
}
}
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> last_U(
+ size_t index = 0) const {
+ return last_U_.template block<number_of_inputs, 1>(0, index);
+ }
+
// Computes the new X and Y given the control input.
void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- if (coefficients().delayed_u) {
- X_ = Update(X(), last_U_);
- UpdateY(last_U_);
- last_U_ = U;
+ if (coefficients().delayed_u > 0) {
+#if defined(__linux__)
+ DCHECK_EQ(static_cast<ssize_t>(coefficients().delayed_u), last_U_.cols());
+#endif
+ X_ = Update(X(), last_U(coefficients().delayed_u - 1));
+ UpdateY(last_U(coefficients().delayed_u - 1));
+ for (int i = coefficients().delayed_u; i > 1; --i) {
+ last_U_.template block<number_of_inputs, 1>(0, i - 1) =
+ last_U_.template block<number_of_inputs, 1>(0, i - 2);
+ }
+ last_U_.template block<number_of_inputs, 1>(0, 0) = U;
} else {
X_ = Update(X(), U);
UpdateY(U);
@@ -210,7 +233,7 @@
private:
Eigen::Matrix<Scalar, number_of_states, 1> X_;
Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
- Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
+ Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic> last_U_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
@@ -310,14 +333,14 @@
// useful for modeling a control loop cycle where you sample, compute, and
// then queue the outputs to be ready to be executed when the next cycle
// happens.
- const bool delayed_u;
+ const size_t delayed_u;
StateFeedbackObserverCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_outputs>
&KalmanGain,
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R,
- bool delayed_u)
+ size_t delayed_u)
: KalmanGain(KalmanGain), Q(Q), R(R), delayed_u(delayed_u) {}
};
@@ -331,7 +354,10 @@
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
&&observers)
- : coefficients_(::std::move(observers)) {}
+ : coefficients_(::std::move(observers)) {
+ last_U_ = Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic>(
+ number_of_inputs, std::max(static_cast<size_t>(1u), coefficients().delayed_u));
+ }
StateFeedbackObserver(StateFeedbackObserver &&other)
: X_hat_(other.X_hat_), last_U_(other.last_U_), index_(other.index_) {
@@ -349,8 +375,9 @@
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
- const Eigen::Matrix<Scalar, number_of_inputs, 1> &last_U() const {
- return last_U_;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> last_U(
+ size_t index = 0) const {
+ return last_U_.template block<number_of_inputs, 1>(0, index);
}
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
@@ -363,9 +390,14 @@
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds /*dt*/) {
- if (plant->coefficients().delayed_u) {
- mutable_X_hat() = plant->Update(X_hat(), last_U_);
- last_U_ = new_u;
+ if (plant->coefficients().delayed_u > 0) {
+ mutable_X_hat() =
+ plant->Update(X_hat(), last_U(coefficients().delayed_u - 1));
+ for (int i = coefficients().delayed_u; i > 1; --i) {
+ last_U_.template block<number_of_inputs, 1>(0, i - 1) =
+ last_U_.template block<number_of_inputs, 1>(0, i - 2);
+ }
+ last_U_.template block<number_of_inputs, 1>(0, 0) = new_u;
} else {
mutable_X_hat() = plant->Update(X_hat(), new_u);
}
@@ -406,7 +438,7 @@
private:
// Internal state estimate.
Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
- Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
+ Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic> last_U_;
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index d6040d1..4a88b14 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -15,9 +15,9 @@
class Catapult(angular_system.AngularSystem):
def __init__(self, params, name="Catapult"):
super(Catapult, self).__init__(params, name)
- # Signal that we have a single cycle output delay to compensate for in
+ # Signal that we have a 2 cycle output delay to compensate for in
# our observer.
- self.delayed_u = True
+ self.delayed_u = 2
self.InitializeState()
@@ -25,9 +25,9 @@
class IntegralCatapult(angular_system.IntegralAngularSystem):
def __init__(self, params, name="IntegralCatapult"):
super(IntegralCatapult, self).__init__(params, name=name)
- # Signal that we have a single cycle output delay to compensate for in
+ # Signal that we have a 2 cycle output delay to compensate for in
# our observer.
- self.delayed_u = True
+ self.delayed_u = 2
self.InitializeState()
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index eb713c2..ebd9ace 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -177,6 +177,7 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/queues:gyro_fbs",
"//third_party:phoenix",
"//third_party:wpilib",
],
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index feb4917..a04d8c9 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -361,10 +361,13 @@
// hardware applies it, we need to run the optimizer for the position at
// the *next* control loop cycle.
- const Eigen::Vector3d next_X =
- catapult_.controller().plant().A() * catapult_.estimated_state() +
- catapult_.controller().plant().B() *
- catapult_.controller().observer().last_U();
+ Eigen::Vector3d next_X = catapult_.estimated_state();
+ for (int i = catapult_.controller().plant().coefficients().delayed_u;
+ i > 1; --i) {
+ next_X = catapult_.controller().plant().A() * next_X +
+ catapult_.controller().plant().B() *
+ catapult_.controller().observer().last_U(i - 1);
+ }
catapult_mpc_.SetState(
next_X.block<2, 1>(0, 0),
diff --git a/y2022/control_loops/superstructure/catapult_plotter.ts b/y2022/control_loops/superstructure/catapult_plotter.ts
index 5736426..90db40b 100644
--- a/y2022/control_loops/superstructure/catapult_plotter.ts
+++ b/y2022/control_loops/superstructure/catapult_plotter.ts
@@ -35,7 +35,7 @@
positionPlot.addMessageLine(status, ['catapult', 'position']).setColor(GREEN).setPointSize(4.0);
positionPlot.addMessageLine(status, ['catapult', 'velocity']).setColor(PINK).setPointSize(1.0);
positionPlot.addMessageLine(status, ['catapult', 'calculated_velocity']).setColor(BROWN).setPointSize(1.0);
- positionPlot.addMessageLine(position, ['catapult', 'pot']).setColor(WHITE).setPointSize(1.0);
+ positionPlot.addMessageLine(position, ['catapult', 'pot']).setColor(WHITE).setPointSize(4.0);
positionPlot.addMessageLine(status, ['catapult', 'estimator_state', 'position']).setColor(CYAN).setPointSize(1.0);
const voltagePlot =
diff --git a/y2022/control_loops/superstructure/led_indicator.cc b/y2022/control_loops/superstructure/led_indicator.cc
index 3fed503..f6b0267 100644
--- a/y2022/control_loops/superstructure/led_indicator.cc
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -15,7 +15,10 @@
"/roborio/aos")),
client_statistics_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ClientStatistics>(
- "/roborio/aos")) {
+ "/roborio/aos")),
+ gyro_reading_fetcher_(
+ event_loop->MakeFetcher<frc971::sensors::GyroReading>(
+ "/drivetrain")) {
led::CANdleConfiguration config;
config.statusLedOffWhenActive = true;
config.disableWhenLOS = false;
@@ -67,6 +70,7 @@
server_statistics_fetcher_.Fetch();
drivetrain_output_fetcher_.Fetch();
client_statistics_fetcher_.Fetch();
+ gyro_reading_fetcher_.Fetch();
// Estopped
if (superstructure_status_fetcher_.get() &&
@@ -82,6 +86,23 @@
return;
}
+ // If the imu gyro readings are not being sent/updated recently
+ if (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time <
+ event_loop->monotonic_now() - frc971::controls::kLoopFrequency) {
+ if (imu_flash_) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(255, 255, 255);
+ }
+
+ if (imu_counter_ % kFlashIterations == 0) {
+ imu_flash_ = !imu_flash_;
+ }
+ imu_counter_++;
+ return;
+ }
+
// Pi disconnected
if ((server_statistics_fetcher_.get() &&
DisconnectedPiServer(*server_statistics_fetcher_)) ||
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index 680b875..355592c 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/queues/gyro_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
@@ -22,6 +23,7 @@
//
// Red: estopped
// Yellow: not zeroed
+ // Flash red/white: imu disconnected
// Flash red/green: pi disconnected
// Purple: driving fast
//
@@ -48,6 +50,7 @@
ctre::phoenix::led::CANdle candle_{0, ""};
+ aos::EventLoop *event_loop;
aos::Fetcher<frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
aos::Fetcher<Status> superstructure_status_fetcher_;
@@ -55,7 +58,10 @@
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
client_statistics_fetcher_;
+ aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+ size_t imu_counter_ = 0;
+ bool imu_flash_ = false;
size_t disconnected_counter_ = 0;
bool disconnected_flash_ = false;
};
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 5dbe7e7..266ebfa 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -244,6 +244,10 @@
.turret_position = turret_.estimated_position(),
.shooting = true});
+ // Dont shoot if the robot is moving faster than this
+ constexpr double kMaxShootSpeed = 1.0;
+ const bool moving_too_fast = std::abs(robot_velocity()) > kMaxShootSpeed;
+
switch (state_) {
case SuperstructureState::IDLE: {
// Only change the turret's goal loading position when idle, to prevent us
@@ -369,8 +373,8 @@
kTurretGoalThreshold;
// Don't open the flippers until the turret's ready: give them as little
- // time to get bumped as possible.
- if (!turret_near_goal || collided) {
+ // time to get bumped as possible. Or moving to fast.
+ if (!turret_near_goal || collided || moving_too_fast) {
break;
}
@@ -527,6 +531,7 @@
status_builder.add_flippers_open(flippers_open_);
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
status_builder.add_fire(fire_);
+ status_builder.add_moving_too_fast(moving_too_fast);
status_builder.add_ready_to_fire(state_ == SuperstructureState::LOADED &&
turret_near_goal && !collided);
status_builder.add_state(state_);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 9288da1..341d315 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -753,7 +753,7 @@
SetEnabled(true);
WaitUntilZeroed();
- SendRobotVelocity(3.0);
+ SendRobotVelocity(1.0);
constexpr double kTurretGoal = 2.0;
{
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 06fddd0..9cf9a5f 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -57,6 +57,8 @@
reseating_in_catapult:bool (id: 13);
// Whether the turret is ready for firing
ready_to_fire:bool (id: 20);
+ // Whether the robot is moving too fast to shoot
+ moving_too_fast:bool (id: 21);
// Whether the catapult was told to fire,
// meaning that the turret and flippers are ready for firing
// and we were asked to fire. Different from fire flag in goal.
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index fe52db7..cfd8952 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -215,7 +215,7 @@
// Keep the catapult return position at the shot one if kCatapultPos is
// pressed
if (data.IsPressed(kCatapultPos)) {
- catapult_return_pos = 0.3;
+ catapult_return_pos = 0.7;
} else {
catapult_return_pos = -0.908;
}
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index e538c2c..3a03a2a 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -61,6 +61,8 @@
namespace chrono = ::std::chrono;
using std::make_unique;
+DEFINE_bool(can_catapult, false, "If true, use CAN to control the catapult.");
+
namespace y2022 {
namespace wpilib {
namespace {
@@ -178,8 +180,15 @@
imu_yaw_rate_input_ = ::std::move(sensor);
imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
}
+ void set_catapult_falcon_1(
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t1,
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t2) {
+ catapult_falcon_1_can_ = ::std::move(t1);
+ catapult_falcon_2_can_ = ::std::move(t2);
+ }
void RunIteration() override {
+ superstructure_reading_->Set(true);
{
auto builder = superstructure_position_sender_.MakeBuilder();
@@ -342,6 +351,13 @@
flipper_arm_right_potentiometer_ = ::std::move(potentiometer);
}
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
+
+ void set_superstructure_reading(
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
+ superstructure_reading_ = superstructure_reading;
+ }
+
void set_intake_encoder_front(::std::unique_ptr<frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
intake_encoder_front_.set_encoder(::std::move(encoder));
@@ -418,6 +434,10 @@
intake_encoder_back_, turret_encoder_, catapult_encoder_;
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
+
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ catapult_falcon_1_can_, catapult_falcon_2_can_;
+
};
class SuperstructureWriter
@@ -425,7 +445,8 @@
public:
SuperstructureWriter(aos::EventLoop *event_loop)
: frc971::wpilib::LoopOutputHandler<superstructure::Output>(
- event_loop, "/superstructure") {}
+ event_loop, "/superstructure"),
+ catapult_reversal_(make_unique<frc::DigitalOutput>(0)) {}
void set_climber_falcon(std::unique_ptr<frc::TalonFX> t) {
climber_falcon_ = std::move(t);
@@ -440,8 +461,8 @@
}
void set_catapult_falcon_1(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t1,
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t2) {
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t1,
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t2) {
catapult_falcon_1_can_ = ::std::move(t1);
catapult_falcon_2_can_ = ::std::move(t2);
@@ -452,12 +473,13 @@
falcon->ConfigStatorCurrentLimit(
{false, Values::kIntakeRollerStatorCurrentLimit(),
Values::kIntakeRollerStatorCurrentLimit(), 0});
- falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_1_General, 5);
- falcon->SetControlFramePeriod(ctre::phoenix::motorcontrol::Control_3_General, 5);
+ falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_1_General, 1);
+ falcon->SetControlFramePeriod(ctre::phoenix::motorcontrol::Control_3_General, 1);
+ falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
+ falcon->ConfigOpenloopRamp(0.0);
+ falcon->ConfigClosedloopRamp(0.0);
+ falcon->ConfigVoltageMeasurementFilter(1);
}
- catapult_falcon_2_can_->Follow(
- *catapult_falcon_1_can_,
- ctre::phoenix::motorcontrol::FollowerType_PercentOutput);
}
void set_intake_falcon_front(::std::unique_ptr<frc::TalonFX> t) {
@@ -514,6 +536,13 @@
transfer_roller_victor_back_ = ::std::move(t);
}
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
+
+ void set_superstructure_reading(
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
+ superstructure_reading_ = superstructure_reading;
+ }
+
private:
void Stop() override {
AOS_LOG(WARNING, "Superstructure output too old.\n");
@@ -534,6 +563,8 @@
if (catapult_falcon_1_can_) {
catapult_falcon_1_can_->Set(
ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ catapult_falcon_2_can_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
}
turret_falcon_->SetDisabled();
}
@@ -554,9 +585,16 @@
if (catapult_falcon_1_) {
WritePwm(output.catapult_voltage(), catapult_falcon_1_.get());
+ superstructure_reading_->Set(false);
+ if (output.catapult_voltage() > 0) {
+ catapult_reversal_->Set(true);
+ } else {
+ catapult_reversal_->Set(false);
+ }
}
if (catapult_falcon_1_can_) {
WriteCan(output.catapult_voltage(), catapult_falcon_1_can_.get());
+ WriteCan(output.catapult_voltage(), catapult_falcon_2_can_.get());
}
WritePwm(-output.turret_voltage(), turret_falcon_.get());
@@ -590,6 +628,8 @@
climber_falcon_;
::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_front_,
transfer_roller_victor_back_;
+
+ std::unique_ptr<frc::DigitalOutput> catapult_reversal_;
};
class CANSensorReader {
@@ -661,12 +701,16 @@
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading =
+ make_unique<frc::DigitalOutput>(25);
+
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop, values);
sensor_reader.set_pwm_trigger(true);
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_intake_encoder_front(make_encoder(3));
sensor_reader.set_intake_front_absolute_pwm(
@@ -734,8 +778,20 @@
superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(8));
superstructure_writer.set_flipper_arms_falcon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
+ superstructure_writer.set_superstructure_reading(superstructure_reading);
- superstructure_writer.set_catapult_falcon_1(make_unique<frc::TalonFX>(9));
+ if (!FLAGS_can_catapult) {
+ superstructure_writer.set_catapult_falcon_1(make_unique<frc::TalonFX>(9));
+ } else {
+ std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> catapult1 =
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(3,
+ "Catapult");
+ std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> catapult2 =
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(4,
+ "Catapult");
+ superstructure_writer.set_catapult_falcon_1(catapult1, catapult2);
+ sensor_reader.set_catapult_falcon_1(catapult1, catapult2);
+ }
AddLoop(&output_event_loop);