Remove pivot, end_effector, and rollers from bot3
This is done because the 3rd robot no longer has them on it,
it won't run without removing them in the code.
Signed-off-by: Maxwell Henderson <maxwell.henderson@mailbox.org>
Change-Id: I3154382fd17e1de5167e4bd242c13318dc227179
diff --git a/y2023_bot3/BUILD b/y2023_bot3/BUILD
index c9258f0..86625da 100644
--- a/y2023_bot3/BUILD
+++ b/y2023_bot3/BUILD
@@ -158,7 +158,6 @@
"//frc971/shooter_interpolation:interpolation",
"//frc971/zeroing:pot_and_absolute_encoder",
"//y2023_bot3/control_loops/drivetrain:polydrivetrain_plants",
- "//y2023_bot3/control_loops/superstructure/pivot_joint:pivot_joint_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2023_bot3/autonomous/autonomous_actor.cc b/y2023_bot3/autonomous/autonomous_actor.cc
index fc794b1..7c722be 100644
--- a/y2023_bot3/autonomous/autonomous_actor.cc
+++ b/y2023_bot3/autonomous/autonomous_actor.cc
@@ -161,9 +161,6 @@
<< "Expect at least one JoystickState message before running auto...";
alliance_ = joystick_state_fetcher_->alliance();
- preloaded_ = false;
- roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
- pivot_goal_ = control_loops::superstructure::PivotGoal::NEUTRAL;
SendSuperstructureGoal();
}
@@ -227,101 +224,25 @@
auto &splines = *charged_up_splines_;
- AOS_LOG(INFO, "Going to preload");
-
- // Tell the superstructure that a cube was preloaded
- if (!WaitForPreloaded()) {
- return;
- }
-
- // Place & Spit firt cube high
- AOS_LOG(INFO, "Moving arm to front high scoring position");
-
- HighScore();
- std::this_thread::sleep_for(chrono::milliseconds(600));
-
- SpitHigh();
- std::this_thread::sleep_for(chrono::milliseconds(600));
-
- StopSpitting();
-
- std::this_thread::sleep_for(chrono::milliseconds(200));
- AOS_LOG(
- INFO, "Placed first cube (HIGH) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- if (FLAGS_one_piece) {
- return;
- }
-
// Drive to second cube
if (!splines[0].WaitForPlan()) {
return;
}
splines[0].Start();
- // Move arm into position to intake cube and intake.
- AOS_LOG(INFO, "Moving arm to back pickup position");
-
- Pickup();
-
- std::this_thread::sleep_for(chrono::milliseconds(500));
- Intake();
-
- AOS_LOG(
- INFO, "Turning on rollers %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- if (!splines[0].WaitForSplineDistanceRemaining(0.02)) {
- return;
- }
-
- AOS_LOG(
- INFO, "Got there %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- // Drive back to grid
if (!splines[1].WaitForPlan()) {
return;
}
splines[1].Start();
- std::this_thread::sleep_for(chrono::milliseconds(600));
- // Place Low
- AOS_LOG(INFO, "Moving arm to front mid scoring position");
-
- MidScore();
-
- std::this_thread::sleep_for(chrono::milliseconds(600));
if (!splines[1].WaitForSplineDistanceRemaining(0.1)) return;
- Spit();
- std::this_thread::sleep_for(chrono::milliseconds(400));
- StopSpitting();
-
- AOS_LOG(
- INFO, "Placed second cube (MID) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
// Drive to third cube
if (!splines[2].WaitForPlan()) {
return;
}
splines[2].Start();
- std::this_thread::sleep_for(chrono::milliseconds(500));
- // Move arm into position to intake cube and intake.
- AOS_LOG(INFO, "Moving arm to back pickup position");
-
- Pickup();
-
- std::this_thread::sleep_for(chrono::milliseconds(250));
- Intake();
-
- AOS_LOG(
- INFO, "Turning on rollers %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
if (!splines[2].WaitForSplineDistanceRemaining(0.02)) {
return;
}
@@ -337,56 +258,17 @@
splines[3].Start();
std::this_thread::sleep_for(chrono::milliseconds(600));
- // Place Low
- AOS_LOG(INFO, "Moving arm to front low scoring position");
-
- LowScore();
-
- std::this_thread::sleep_for(chrono::milliseconds(600));
if (!splines[3].WaitForSplineDistanceRemaining(0.1)) return;
-
- Spit();
- std::this_thread::sleep_for(chrono::milliseconds(600));
- StopSpitting();
-
- AOS_LOG(
- INFO, "Placed low cube (LOW) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
}
// Charged Up Place and Mobility Autonomous (middle)
void AutonomousActor::ChargedUpMiddle() {
- aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
-
CHECK(charged_up_middle_splines_);
auto &splines = *charged_up_middle_splines_;
AOS_LOG(INFO, "Going to preload");
- // Tell the superstructure that a cube was preloaded
- if (!WaitForPreloaded()) {
- return;
- }
-
- // Place & Spit firt cube mid
- AOS_LOG(INFO, "Moving arm to front mid scoring position");
-
- MidScore();
- std::this_thread::sleep_for(chrono::milliseconds(300));
-
- Spit();
- std::this_thread::sleep_for(chrono::milliseconds(300));
-
- StopSpitting();
-
- std::this_thread::sleep_for(chrono::milliseconds(100));
- AOS_LOG(
- INFO, "Placed first cube (Mid) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- // Drive to second cube
-
if (!splines[0].WaitForPlan()) {
return;
}
@@ -399,83 +281,11 @@
control_loops::superstructure::Goal::Builder superstructure_builder =
builder.MakeBuilder<control_loops::superstructure::Goal>();
- superstructure_builder.add_pivot_goal(pivot_goal_);
- superstructure_builder.add_roller_goal(roller_goal_);
- superstructure_builder.add_preloaded_with_cube(preloaded_);
-
if (builder.Send(superstructure_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
-[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
- set_preloaded(true);
- SendSuperstructureGoal();
-
- ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
- event_loop()->monotonic_now(),
- ActorBase::kLoopOffset);
-
- bool loaded = false;
- while (!loaded) {
- if (ShouldCancel()) {
- return false;
- }
-
- phased_loop.SleepUntilNext();
- superstructure_status_fetcher_.Fetch();
- CHECK(superstructure_status_fetcher_.get() != nullptr);
-
- loaded = (superstructure_status_fetcher_->end_effector_state() ==
- control_loops::superstructure::EndEffectorState::LOADED);
- }
-
- set_preloaded(false);
- SendSuperstructureGoal();
-
- return true;
-}
-
-void AutonomousActor::HighScore() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_HIGH_FRONT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::MidScore() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_MID_FRONT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::LowScore() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_LOW_FRONT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::Spit() {
- set_roller_goal(control_loops::superstructure::RollerGoal::SPIT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::SpitHigh() {
- set_roller_goal(control_loops::superstructure::RollerGoal::SPIT_HIGH);
- SendSuperstructureGoal();
-}
-
-void AutonomousActor::StopSpitting() {
- set_roller_goal(control_loops::superstructure::RollerGoal::IDLE);
- SendSuperstructureGoal();
-}
-void AutonomousActor::Intake() {
- set_roller_goal(control_loops::superstructure::RollerGoal::INTAKE_CUBE);
- SendSuperstructureGoal();
-}
-
-void AutonomousActor::Pickup() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::PICKUP_BACK);
- SendSuperstructureGoal();
-}
-
-void AutonomousActor::Neutral() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::NEUTRAL);
- SendSuperstructureGoal();
-}
-
} // namespace autonomous
} // namespace y2023_bot3
diff --git a/y2023_bot3/autonomous/autonomous_actor.h b/y2023_bot3/autonomous/autonomous_actor.h
index ad33eed..bc0a35c 100644
--- a/y2023_bot3/autonomous/autonomous_actor.h
+++ b/y2023_bot3/autonomous/autonomous_actor.h
@@ -11,8 +11,6 @@
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-// TODO<FILIP>: Add NEUTRAL pivot pose.
-
namespace y2023_bot3 {
namespace autonomous {
@@ -24,32 +22,8 @@
const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
- void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
-
- void set_pivot_goal(
- control_loops::superstructure::PivotGoal requested_pivot_goal) {
- pivot_goal_ = requested_pivot_goal;
- }
-
- void set_roller_goal(
- control_loops::superstructure::RollerGoal requested_roller_goal) {
- roller_goal_ = requested_roller_goal;
- }
-
void SendSuperstructureGoal();
- [[nodiscard]] bool WaitForPreloaded();
-
- void HighScore();
- void MidScore();
- void LowScore();
- void Spit();
- void SpitHigh();
- void StopSpitting();
- void Pickup();
- void Intake();
- void Neutral();
-
void Reset();
void SendStartingPosition(const Eigen::Vector3d &start);
@@ -76,11 +50,6 @@
std::optional<Eigen::Vector3d> starting_position_;
- bool preloaded_ = false;
-
- control_loops::superstructure::PivotGoal pivot_goal_;
- control_loops::superstructure::RollerGoal roller_goal_;
-
aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
aos::Fetcher<y2023_bot3::control_loops::superstructure::Status>
superstructure_status_fetcher_;
diff --git a/y2023_bot3/constants.cc b/y2023_bot3/constants.cc
index 0a99b94..7f77d4d 100644
--- a/y2023_bot3/constants.cc
+++ b/y2023_bot3/constants.cc
@@ -12,7 +12,6 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint/integral_pivot_joint_plant.h"
namespace y2023_bot3 {
namespace constants {
@@ -21,41 +20,13 @@
LOG(INFO) << "creating a Constants for team: " << team;
Values r;
- auto *const pivot_joint = &r.pivot_joint;
-
- r.pivot_joint_flipped = true;
-
- pivot_joint->subsystem_params.zeroing_voltage = 3.0;
- pivot_joint->subsystem_params.operating_voltage = 12.0;
- pivot_joint->subsystem_params.zeroing_profile_params = {0.5, 3.0};
- pivot_joint->subsystem_params.default_profile_params = {5.0, 5.0};
- pivot_joint->subsystem_params.range = Values::kPivotJointRange();
- pivot_joint->subsystem_params.make_integral_loop =
- control_loops::superstructure::pivot_joint::MakeIntegralPivotJointLoop;
- pivot_joint->subsystem_params.zeroing_constants.average_filter_size =
- Values::kZeroingSampleSize;
- pivot_joint->subsystem_params.zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kPivotJointEncoderRatio();
- pivot_joint->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
- pivot_joint->subsystem_params.zeroing_constants.moving_buffer_size = 20;
- pivot_joint->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
switch (team) {
// A set of constants for tests.
case 1:
-
- pivot_joint->potentiometer_offset = 0.0;
-
- pivot_joint->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
-
break;
case kThirdRobotTeamNumber:
- pivot_joint->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.564786179025525;
-
- pivot_joint->potentiometer_offset = 0.304569457401797 + 2.66972311194163;
break;
default:
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index ba7267c..60fe404 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -10,7 +10,6 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint/pivot_joint_plant.h"
namespace y2023_bot3 {
namespace constants {
@@ -40,12 +39,6 @@
static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
- static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
- static constexpr double kRollerStatorCurrentLimit() { return 100.0; }
-
- static constexpr double kPivotSupplyCurrentLimit() { return 40.0; }
- static constexpr double kPivotStatorCurrentLimit() { return 200.0; }
-
// timeout to ensure code doesn't get stuck after releasing the "intake"
// button
static constexpr std::chrono::milliseconds kExtraIntakingTime() {
@@ -63,51 +56,6 @@
return (rotations * (2.0 * M_PI)) *
control_loops::drivetrain::kHighOutputRatio;
}
-
- // Pivot Joint
- static constexpr double kPivotJointEncoderCountsPerRevolution() {
- return 4096.0;
- }
-
- static constexpr double kPivotJointEncoderRatio() {
- return (24.0 / 64.0) * (15.0 / 60.0);
- }
-
- static constexpr double kPivotJointPotRatio() {
- return (24.0 / 64.0) * (15.0 / 60.0);
- }
-
- static constexpr double kPivotJointPotRadiansPerVolt() {
- return kPivotJointPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
- }
-
- static constexpr double kMaxPivotJointEncoderPulsesPerSecond() {
- return control_loops::superstructure::pivot_joint::kFreeSpeed /
- (2.0 * M_PI) *
- control_loops::superstructure::pivot_joint::kOutputRatio /
- kPivotJointEncoderRatio() * kPivotJointEncoderCountsPerRevolution();
- }
-
- static constexpr ::frc971::constants::Range kPivotJointRange() {
- return ::frc971::constants::Range{
- .lower_hard = -1.78879503977269, // Back Hard
- .upper_hard = 1.76302285774785, // Front Hard
- .lower = -1.77156498873494, // Back Soft
- .upper = 1.76555657862879, // Front Soft
- };
- }
-
- struct PotAndAbsEncoderConstants {
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- subsystem_params;
- double potentiometer_offset;
- };
-
- PotAndAbsEncoderConstants pivot_joint;
-
- bool pivot_joint_flipped;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023_bot3/control_loops/python/BUILD b/y2023_bot3/control_loops/python/BUILD
index 2753020..fe7666f 100644
--- a/y2023_bot3/control_loops/python/BUILD
+++ b/y2023_bot3/control_loops/python/BUILD
@@ -55,19 +55,3 @@
visibility = ["//visibility:public"],
deps = ["//y2023_bot3/control_loops:python_init"],
)
-
-py_binary(
- name = "pivot_joint",
- srcs = [
- "pivot_joint.py",
- ],
- legacy_create_init = False,
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- ":python_init",
- "//frc971/control_loops/python:angular_system",
- "//frc971/control_loops/python:controls",
- "@pip//glog",
- "@pip//python_gflags",
- ],
-)
diff --git a/y2023_bot3/control_loops/python/pivot_joint.py b/y2023_bot3/control_loops/python/pivot_joint.py
deleted file mode 100644
index baea920..0000000
--- a/y2023_bot3/control_loops/python/pivot_joint.py
+++ /dev/null
@@ -1,60 +0,0 @@
-#!/usr/bin/python3
-
-from aos.util.trapezoid_profile import TrapezoidProfile
-from frc971.control_loops.python import control_loop
-from frc971.control_loops.python import angular_system
-from frc971.control_loops.python import controls
-import numpy
-import sys
-from matplotlib import pylab
-import gflags
-import glog
-
-FLAGS = gflags.FLAGS
-
-try:
- gflags.DEFINE_bool("plot", False, "If true, plot the loop response.")
-except gflags.DuplicateFlagError:
- pass
-
-kPivotJoint = angular_system.AngularSystemParams(
- name="PivotJoint",
- motor=control_loop.Falcon(),
- G=(14 / 50) * (24 / 64) * (24 / 64) * (15 / 60),
- # Use parallel axis theorem to get the moment of inertia around
- # the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
- J=(0.13372 * 2.00),
- q_pos=0.80,
- q_vel=80.0,
- kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=1.5,
- kalman_r_position=0.05,
- radius=5.71 * 0.0254,
-)
-
-
-def main(argv):
- if FLAGS.plot:
- R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
- angular_system.PlotKick(kPivotJoint, R)
- angular_system.PlotMotion(kPivotJoint, R)
- return
-
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal(
- "Expected .h file name and .cc file name for the pivot joint and integral pivot joint."
- )
- else:
- namespaces = [
- "y2023_bot3", "control_loops", "superstructure", "pivot_joint"
- ]
- angular_system.WriteAngularSystem(kPivotJoint, argv[1:3], argv[3:5],
- namespaces)
-
-
-if __name__ == "__main__":
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index 8630ef9..fe41eed 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -61,24 +61,6 @@
)
cc_library(
- name = "end_effector",
- srcs = [
- "end_effector.cc",
- ],
- hdrs = [
- "end_effector.h",
- ],
- deps = [
- ":superstructure_goal_fbs",
- ":superstructure_status_fbs",
- "//aos/events:event_loop",
- "//aos/time",
- "//frc971/control_loops:control_loop",
- "//y2023_bot3:constants",
- ],
-)
-
-cc_library(
name = "superstructure_lib",
srcs = [
"superstructure.cc",
@@ -89,8 +71,6 @@
data = [
],
deps = [
- ":end_effector",
- ":pivot_joint",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -189,21 +169,3 @@
"//aos/network:team_number",
],
)
-
-cc_library(
- name = "pivot_joint",
- srcs = [
- "pivot_joint.cc",
- ],
- hdrs = [
- "pivot_joint.h",
- ],
- deps = [
- ":superstructure_goal_fbs",
- ":superstructure_status_fbs",
- "//aos/events:event_loop",
- "//aos/time",
- "//frc971/control_loops:control_loop",
- "//y2023_bot3:constants",
- ],
-)
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.cc b/y2023_bot3/control_loops/superstructure/end_effector.cc
deleted file mode 100644
index 665aa9d..0000000
--- a/y2023_bot3/control_loops/superstructure/end_effector.cc
+++ /dev/null
@@ -1,120 +0,0 @@
-#include "y2023_bot3/control_loops/superstructure/end_effector.h"
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-using ::aos::monotonic_clock;
-
-EndEffector::EndEffector()
- : state_(EndEffectorState::IDLE), timer_(aos::monotonic_clock::min_time) {}
-
-void EndEffector::RunIteration(
- const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
- bool beambreak_status, double *roller_voltage, bool preloaded_with_cube) {
- *roller_voltage = 0.0;
-
- // If we started off preloaded, skip to the loaded state.
- // Make sure we weren't already there just in case.
- if (preloaded_with_cube) {
- switch (state_) {
- case EndEffectorState::IDLE:
- case EndEffectorState::INTAKING:
- state_ = EndEffectorState::LOADED;
- break;
- case EndEffectorState::LOADED:
- break;
- case EndEffectorState::SPITTING:
- break;
- case EndEffectorState::SPITTING_MID:
- break;
- case EndEffectorState::SPITTING_HIGH:
- break;
- }
- }
-
- if (roller_goal == RollerGoal::SPIT) {
- state_ = EndEffectorState::SPITTING;
- }
-
- if (roller_goal == RollerGoal::SPIT_MID) {
- state_ = EndEffectorState::SPITTING_MID;
- }
-
- if (roller_goal == RollerGoal::SPIT_HIGH) {
- state_ = EndEffectorState::SPITTING_HIGH;
- }
-
- switch (state_) {
- case EndEffectorState::IDLE:
- // If idle and intake requested, intake
- if (roller_goal == RollerGoal::INTAKE_CUBE) {
- state_ = EndEffectorState::INTAKING;
- timer_ = timestamp;
- }
- break;
- case EndEffectorState::INTAKING:
- // If intaking and beam break is not triggered, keep intaking
- if (roller_goal == RollerGoal::INTAKE_CUBE) {
- timer_ = timestamp;
- }
-
- if (beambreak_status) {
- // Beam has been broken, switch to loaded.
- state_ = EndEffectorState::LOADED;
- break;
- } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
- // Intaking failed, waited 1 second with no beambreak
- state_ = EndEffectorState::IDLE;
- break;
- }
-
- *roller_voltage = kRollerCubeSuckVoltage();
-
- break;
- case EndEffectorState::LOADED:
- timer_ = timestamp;
- if (!preloaded_with_cube && !beambreak_status) {
- state_ = EndEffectorState::INTAKING;
- break;
- }
-
- break;
-
- case EndEffectorState::SPITTING:
- *roller_voltage = kRollerCubeSpitVoltage();
-
- if (roller_goal == RollerGoal::IDLE) {
- state_ = EndEffectorState::IDLE;
- }
-
- break;
-
- case EndEffectorState::SPITTING_MID:
- *roller_voltage = kRollerCubeSpitMidVoltage();
-
- if (roller_goal == RollerGoal::IDLE) {
- state_ = EndEffectorState::IDLE;
- }
-
- break;
-
- case EndEffectorState::SPITTING_HIGH:
- *roller_voltage = kRollerCubeSpitHighVoltage();
-
- if (roller_goal == RollerGoal::IDLE) {
- state_ = EndEffectorState::IDLE;
- }
- break;
- }
-}
-
-void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
\ No newline at end of file
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.h b/y2023_bot3/control_loops/superstructure/end_effector.h
deleted file mode 100644
index be0abc0..0000000
--- a/y2023_bot3/control_loops/superstructure/end_effector.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
-#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class EndEffector {
- public:
- static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
- static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
- static constexpr double kRollerCubeSpitMidVoltage() { return 5.0; }
- static constexpr double kRollerCubeSpitHighVoltage() { return 6.37; }
-
- EndEffector();
- void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
- RollerGoal roller_goal, bool beambreak_status,
- double *intake_roller_voltage, bool preloaded_with_cube);
- EndEffectorState state() const { return state_; }
- void Reset();
-
- private:
- EndEffectorState state_;
-
- // variable which records the last time at which "intake" button was pressed
- aos::monotonic_clock::time_point timer_;
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
-
-#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
deleted file mode 100644
index be7e646..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ /dev/null
@@ -1,83 +0,0 @@
-#include "pivot_joint.h"
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-PivotJoint::PivotJoint(std::shared_ptr<const constants::Values> values)
- : pivot_joint_(values->pivot_joint.subsystem_params) {}
-
-flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-PivotJoint::RunIteration(PivotGoal goal, double *output,
- const frc971::PotAndAbsolutePosition *position,
- flatbuffers::FlatBufferBuilder *status_fbb) {
- double pivot_goal = 0;
- switch (goal) {
- case PivotGoal::NEUTRAL:
- pivot_goal = 0;
- break;
-
- case PivotGoal::PICKUP_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::PICKUP_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_MID_BACK:
- pivot_goal = -0.763222056740831;
- break;
-
- case PivotGoal::SCORE_HIGH_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_HIGH_BACK:
- pivot_goal = -0.763222056740831;
- break;
- }
-
- flatbuffers::Offset<
- frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>
- pivot_joint_offset = frc971::control_loops::
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *status_fbb, pivot_goal,
- frc971::CreateProfileParameters(*status_fbb, 5.0, 20.0));
-
- status_fbb->Finish(pivot_joint_offset);
-
- flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- pivot_joint_status_offset = pivot_joint_.Iterate(
- flatbuffers::GetRoot<frc971::control_loops::
- StaticZeroingSingleDOFProfiledSubsystemGoal>(
- status_fbb->GetBufferPointer()),
- position, output, status_fbb);
-
- return pivot_joint_status_offset;
-}
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.h b/y2023_bot3/control_loops/superstructure/pivot_joint.h
deleted file mode 100644
index 1eff122..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
-#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class PivotJoint {
- using PotAndAbsoluteEncoderSubsystem =
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
-
- public:
- PivotJoint(std::shared_ptr<const constants::Values> values);
-
- flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- RunIteration(PivotGoal goal, double *output,
- const frc971::PotAndAbsolutePosition *position,
- flatbuffers::FlatBufferBuilder *status_fbb);
-
- bool zeroed() const { return pivot_joint_.zeroed(); }
-
- bool estopped() const { return pivot_joint_.estopped(); }
-
- // variable which records the last time at which "intake" button was pressed
- aos::monotonic_clock::time_point timer_;
-
- private:
- PotAndAbsoluteEncoderSubsystem pivot_joint_;
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
-
-#endif // Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD b/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
deleted file mode 100644
index ae413ab..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
+++ /dev/null
@@ -1,34 +0,0 @@
-package(default_visibility = ["//y2023_bot3:__subpackages__"])
-
-genrule(
- name = "genrule_pivot_joint",
- outs = [
- "pivot_joint_plant.h",
- "pivot_joint_plant.cc",
- "integral_pivot_joint_plant.h",
- "integral_pivot_joint_plant.cc",
- ],
- cmd = "$(location //y2023_bot3/control_loops/python:pivot_joint) $(OUTS)",
- target_compatible_with = ["@platforms//os:linux"],
- tools = [
- "//y2023_bot3/control_loops/python:pivot_joint",
- ],
-)
-
-cc_library(
- name = "pivot_joint_plants",
- srcs = [
- "integral_pivot_joint_plant.cc",
- "pivot_joint_plant.cc",
- ],
- hdrs = [
- "integral_pivot_joint_plant.h",
- "pivot_joint_plant.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/control_loops:hybrid_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- ],
-)
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 6061a76..e77245e 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -23,9 +23,7 @@
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- values_(values),
- end_effector_(),
- pivot_joint_(values) {
+ values_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -33,41 +31,23 @@
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
- const monotonic_clock::time_point timestamp =
- event_loop()->context().monotonic_event_time;
+ (void)unsafe_goal;
+ (void)position;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
- end_effector_.Reset();
}
OutputT output_struct;
- end_effector_.RunIteration(
- timestamp,
- unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
- position->end_effector_cube_beam_break(), &output_struct.roller_voltage,
- unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cube() : false);
-
- flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- pivot_joint_offset = pivot_joint_.RunIteration(
- unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
- : PivotGoal::NEUTRAL,
- output != nullptr ? &(output_struct.pivot_joint_voltage) : nullptr,
- position->pivot_joint_position(), status->fbb());
-
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(pivot_joint_.zeroed());
- status_builder.add_estopped(pivot_joint_.estopped());
- status_builder.add_pivot_joint(pivot_joint_offset);
- status_builder.add_end_effector_state(end_effector_.state());
-
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
+ status_builder.add_zeroed(true);
+
(void)status->Send(status_builder.Finish());
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index efc95a8..d0cd0db 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -9,8 +9,6 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023_bot3/constants.h"
#include "y2023_bot3/constants/constants_generated.h"
-#include "y2023_bot3/control_loops/superstructure/end_effector.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_position_generated.h"
@@ -34,8 +32,6 @@
double robot_velocity() const;
- inline const EndEffector &end_effector() const { return end_effector_; }
-
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -46,12 +42,8 @@
std::optional<double> LateralOffsetForTimeOfFlight(double reading);
std::shared_ptr<const constants::Values> values_;
- EndEffector end_effector_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
-
- PivotJoint pivot_joint_;
-
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
index d2b8e0b..326a7b0 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -2,30 +2,7 @@
namespace y2023_bot3.control_loops.superstructure;
-enum PivotGoal: ubyte {
- NEUTRAL = 0,
- PICKUP_FRONT = 1,
- PICKUP_BACK = 2,
- SCORE_LOW_FRONT = 3,
- SCORE_LOW_BACK = 4,
- SCORE_MID_FRONT = 5,
- SCORE_MID_BACK = 6,
- SCORE_HIGH_FRONT = 7,
- SCORE_HIGH_BACK = 8,
-}
-
-enum RollerGoal: ubyte {
- IDLE = 0,
- INTAKE_CUBE = 1,
- SPIT = 2,
- SPIT_MID = 3,
- SPIT_HIGH = 4,
-}
-
table Goal {
- pivot_goal:PivotGoal (id: 0);
- roller_goal:RollerGoal (id: 1);
- preloaded_with_cube:bool (id: 2);
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 10f8f6e..370ec17 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -31,11 +31,6 @@
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
-using PotAndAbsoluteEncoderSimulator =
- frc971::control_loops::SubsystemSimulator<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
- Superstructure::PotAndAbsoluteEncoderSubsystem::State,
- constants::Values::PotAndAbsEncoderConstants>;
// Class which simulates the superstructure and sends out queue messages with
// the position.
@@ -50,15 +45,7 @@
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<Output>("/superstructure")),
- pivot_joint_(new CappedTestPlant(pivot_joint::MakePivotJointPlant()),
- PositionSensorSimulator(
- values->pivot_joint.subsystem_params.zeroing_constants
- .one_revolution_distance),
- values->pivot_joint, constants::Values::kPivotJointRange(),
- values->pivot_joint.subsystem_params.zeroing_constants
- .measured_absolute_position,
- dt) {
+ event_loop_->MakeFetcher<Output>("/superstructure")) {
(void)values;
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
@@ -66,10 +53,6 @@
if (!first_) {
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
-
- pivot_joint_.Simulate(
- superstructure_output_fetcher_->pivot_joint_voltage(),
- superstructure_status_fetcher_->pivot_joint());
}
first_ = false;
SendPositionMessage();
@@ -82,38 +65,20 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
- frc971::PotAndAbsolutePosition::Builder pivot_joint_builder =
- builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
- flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_joint_offset =
- pivot_joint_.encoder()->GetSensorValues(&pivot_joint_builder);
-
Position::Builder position_builder = builder.MakeBuilder<Position>();
- position_builder.add_end_effector_cube_beam_break(
- end_effector_cube_beam_break_);
- position_builder.add_pivot_joint_position(pivot_joint_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
- void set_end_effector_cube_beam_break(bool triggered) {
- end_effector_cube_beam_break_ = triggered;
- }
-
- PotAndAbsoluteEncoderSimulator *pivot_joint() { return &pivot_joint_; }
-
private:
::aos::EventLoop *event_loop_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
- bool end_effector_cube_beam_break_ = false;
-
::aos::Sender<Position> superstructure_position_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
- PotAndAbsoluteEncoderSimulator pivot_joint_;
-
bool first_ = true;
};
@@ -167,51 +132,6 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
-
- if (superstructure_goal_fetcher_->has_pivot_goal()) {
- double pivot_goal = 0.0;
-
- switch (superstructure_goal_fetcher_->pivot_goal()) {
- case PivotGoal::NEUTRAL:
- pivot_goal = 0;
- break;
-
- case PivotGoal::PICKUP_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::PICKUP_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_MID_BACK:
- pivot_goal = -0.763222056740831;
- break;
-
- case PivotGoal::SCORE_HIGH_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_HIGH_BACK:
- pivot_goal = -0.763222056740831;
- break;
- }
-
- EXPECT_NEAR(pivot_goal,
- superstructure_status_fetcher_->pivot_joint()->position(), 3);
- }
}
void CheckIfZeroed() {
@@ -323,323 +243,6 @@
CheckIfZeroed();
}
-TEST_F(SuperstructureTest, PivotGoal) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- PivotGoal pivot_goal = PivotGoal::PICKUP_FRONT;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::PICKUP_BACK;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_LOW_FRONT;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_LOW_BACK;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_MID_FRONT;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_MID_BACK;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::NEUTRAL;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-}
-
-TEST_F(SuperstructureTest, EndEffectorGoal) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- double spit_voltage = EndEffector::kRollerCubeSpitVoltage();
- double suck_voltage = EndEffector::kRollerCubeSuckVoltage();
-
- RollerGoal roller_goal = RollerGoal::INTAKE_CUBE;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(roller_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
- superstructure_plant_.set_end_effector_cube_beam_break(false);
-
- // This makes sure that we intake as normal when
- // requesting intake.
- RunFor(constants::Values::kExtraIntakingTime());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::INTAKING);
-
- superstructure_plant_.set_end_effector_cube_beam_break(true);
-
- // Checking that after the beambreak is set once intaking that the
- // state changes to LOADED.
- RunFor(dt());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::LOADED);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
- superstructure_plant_.set_end_effector_cube_beam_break(false);
-
- // Checking that it's going back to intaking because we lost the
- // beambreak sensor.
- RunFor(dt() * 2);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::INTAKING);
-
- // Checking that we go back to idle after beambreak is lost and we
- // set our goal to idle.
- RunFor(dt() * 2 + constants::Values::kExtraIntakingTime());
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::IDLE);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(roller_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- // Going through intake -> loaded -> spitting
- // Making sure that it's intaking normally.
- RunFor(constants::Values::kExtraIntakingTime());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::INTAKING);
-
- superstructure_plant_.set_end_effector_cube_beam_break(true);
-
- // Checking that it's loaded once beambreak is sensing something.
- RunFor(dt());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::LOADED);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(RollerGoal::SPIT);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
- superstructure_plant_.set_end_effector_cube_beam_break(true);
- // Checking that it stays spitting until 2 seconds after the
- // beambreak is lost.
- RunFor(dt() * 10);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), spit_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::SPITTING);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- // Checking that it goes to idle after it's given time to stop spitting.
- RunFor(dt() * 3);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::IDLE);
-}
-
-// Test that we are able to signal that the cube was preloaded
-TEST_F(SuperstructureTest, Preloaded) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_preloaded_with_cube(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(dt());
-
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::LOADED);
-}
-
-// Tests that the end effector does nothing when the goal is to remain
-// still.
-TEST_F(SuperstructureTest, DoesNothing) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
- RunFor(chrono::seconds(10));
- VerifyNearGoal();
-
- EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
-}
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
SetEnabled(true);
@@ -649,8 +252,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Give it a lot of time to get there.
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
index 652d247..98dbcf7 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -1,10 +1,6 @@
namespace y2023_bot3.control_loops.superstructure;
table Output {
- // Pivot joint voltage
- pivot_joint_voltage:double (id: 0);
- // Roller voltage on the end effector (positive is spitting, negative is sucking)
- roller_voltage:double (id: 1);
}
root_type Output;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
index a629e56..28e4849 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -5,14 +5,6 @@
namespace y2023_bot3.control_loops.superstructure;
table Position {
- // The position of the pivot joint in radians
- pivot_joint_position:frc971.PotAndAbsolutePosition (id: 0);
-
- // If this is true, the cube beam break is triggered.
- end_effector_cube_beam_break:bool (id: 1);
-
- // Roller falcon data
- roller_falcon:frc971.control_loops.CANFalcon (id: 2);
}
root_type Position;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index 4492cb9..b36e0da 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -3,35 +3,12 @@
namespace y2023_bot3.control_loops.superstructure;
-enum EndEffectorState : ubyte {
- // Not doing anything
- IDLE = 0,
- // Intaking the game object into the end effector
- INTAKING = 1,
- // The game object is loaded into the end effector
- LOADED = 2,
- // Waiting for the arm to be at shooting goal and then telling the
- // end effector to spit
- SPITTING = 3,
- // Waiting for the arm to be at MID shooting goal and then telling the
- // end effector to spit mid
- SPITTING_MID = 4,
- // Waiting for the arm to be at HIGH shooting goal and then telling the
- // end effector to spit mid
- SPITTING_HIGH = 5
-}
-
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
-
- // The current state of the pivot.
- pivot_joint:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
-
- end_effector_state:EndEffectorState (id: 3);
}
root_type Status;
diff --git a/y2023_bot3/joystick_reader.cc b/y2023_bot3/joystick_reader.cc
index fd32652..4c42bdc 100644
--- a/y2023_bot3/joystick_reader.cc
+++ b/y2023_bot3/joystick_reader.cc
@@ -31,8 +31,6 @@
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
using Side = frc971::control_loops::drivetrain::RobotSide;
-using y2023_bot3::control_loops::superstructure::PivotGoal;
-using y2023_bot3::control_loops::superstructure::RollerGoal;
namespace y2023_bot3 {
namespace input {
@@ -90,33 +88,6 @@
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
- RollerGoal roller_goal = RollerGoal::IDLE;
- PivotGoal pivot_goal = PivotGoal::NEUTRAL;
-
- if (data.IsPressed(kSpit)) {
- roller_goal = RollerGoal::SPIT;
- } else if (data.IsPressed(kSpitHigh)) {
- roller_goal = RollerGoal::SPIT_HIGH;
- }
-
- if (data.IsPressed(kScore)) {
- pivot_goal = PivotGoal::SCORE_LOW_FRONT;
- } else if (data.IsPressed(kScoreBack)) {
- pivot_goal = PivotGoal::SCORE_LOW_BACK;
- } else if (data.IsPressed(kScoreMid)) {
- pivot_goal = PivotGoal::SCORE_MID_FRONT;
- } else if (data.IsPressed(kScoreMidBack)) {
- pivot_goal = PivotGoal::SCORE_MID_BACK;
- } else if (data.IsPressed(kPickup)) {
- pivot_goal = PivotGoal::PICKUP_FRONT;
- roller_goal = RollerGoal::INTAKE_CUBE;
- } else if (data.IsPressed(kPickupBack)) {
- pivot_goal = PivotGoal::PICKUP_BACK;
- roller_goal = RollerGoal::INTAKE_CUBE;
- }
-
- superstructure_goal_builder.add_roller_goal(roller_goal);
- superstructure_goal_builder.add_pivot_goal(pivot_goal);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
@@ -146,4 +117,4 @@
event_loop.Run();
return 0;
-}
\ No newline at end of file
+}
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index e91f6e7..9ce8253 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -88,13 +88,8 @@
control_loops::drivetrain::kWheelRadius;
}
-double pivot_pot_translate(double voltage) {
- return voltage * Values::kPivotJointPotRadiansPerVolt();
-}
-
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
Values::kMaxDrivetrainEncoderPulsesPerSecond(),
- Values::kMaxPivotJointEncoderPulsesPerSecond(),
});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -180,62 +175,6 @@
PrintConfigs();
}
- void WriteRollerConfigs() {
- ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
- current_limits.StatorCurrentLimit =
- constants::Values::kRollerStatorCurrentLimit();
- current_limits.StatorCurrentLimitEnable = true;
- current_limits.SupplyCurrentLimit =
- constants::Values::kRollerSupplyCurrentLimit();
- current_limits.SupplyCurrentLimitEnable = true;
-
- ctre::phoenix6::configs::MotorOutputConfigs output_configs;
- output_configs.NeutralMode =
- ctre::phoenix6::signals::NeutralModeValue::Brake;
- output_configs.DutyCycleNeutralDeadband = 0;
-
- ctre::phoenix6::configs::TalonFXConfiguration configuration;
- configuration.CurrentLimits = current_limits;
- configuration.MotorOutput = output_configs;
-
- ctre::phoenix::StatusCode status =
- talon_.GetConfigurator().Apply(configuration);
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- PrintConfigs();
- }
-
- void WritePivotConfigs() {
- ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
- current_limits.StatorCurrentLimit =
- constants::Values::kPivotStatorCurrentLimit();
- current_limits.StatorCurrentLimitEnable = true;
- current_limits.SupplyCurrentLimit =
- constants::Values::kPivotSupplyCurrentLimit();
- current_limits.SupplyCurrentLimitEnable = true;
-
- ctre::phoenix6::configs::MotorOutputConfigs output_configs;
- output_configs.NeutralMode =
- ctre::phoenix6::signals::NeutralModeValue::Brake;
- output_configs.DutyCycleNeutralDeadband = 0;
-
- ctre::phoenix6::configs::TalonFXConfiguration configuration;
- configuration.CurrentLimits = current_limits;
- configuration.MotorOutput = output_configs;
-
- ctre::phoenix::StatusCode status =
- talon_.GetConfigurator().Apply(configuration);
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- PrintConfigs();
- }
-
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
@@ -302,8 +241,7 @@
can_position_sender_(
event_loop
->MakeSender<frc971::control_loops::drivetrain::CANPosition>(
- "/drivetrain")),
- roller_falcon_data_(std::nullopt) {
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(40);
event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
@@ -318,20 +256,11 @@
void set_falcons(std::shared_ptr<Falcon> right_front,
std::shared_ptr<Falcon> right_back,
std::shared_ptr<Falcon> left_front,
- std::shared_ptr<Falcon> left_back,
- std::shared_ptr<Falcon> roller_falcon,
- std::shared_ptr<Falcon> pivot_falcon) {
+ std::shared_ptr<Falcon> left_back) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
left_front_ = std::move(left_front);
left_back_ = std::move(left_back);
- roller_falcon_ = std::move(roller_falcon);
- pivot_falcon_ = std::move(pivot_falcon);
- }
-
- std::optional<frc971::control_loops::CANFalconT> roller_falcon_data() {
- std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
- return roller_falcon_data_;
}
private:
@@ -346,8 +275,7 @@
auto builder = can_position_sender_.MakeBuilder();
- for (auto falcon : {right_front_, right_back_, left_front_, left_back_,
- roller_falcon_, pivot_falcon_}) {
+ for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
falcon->RefreshNontimesyncedSignals();
}
@@ -374,21 +302,6 @@
can_position_builder.add_status(static_cast<int>(status));
builder.CheckOk(builder.Send(can_position_builder.Finish()));
-
- {
- std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
- frc971::control_loops::CANFalconT roller_falcon_data;
- roller_falcon_data.id = roller_falcon_->device_id();
- roller_falcon_data.supply_current = roller_falcon_->supply_current();
- roller_falcon_data.torque_current = -roller_falcon_->torque_current();
- roller_falcon_data.supply_voltage = roller_falcon_->supply_voltage();
- roller_falcon_data.device_temp = roller_falcon_->device_temp();
- roller_falcon_data.position = -roller_falcon_->position();
- roller_falcon_data.duty_cycle = roller_falcon_->duty_cycle();
- roller_falcon_data_ =
- std::make_optional<frc971::control_loops::CANFalconT>(
- roller_falcon_data);
- }
}
aos::EventLoop *event_loop_;
@@ -397,12 +310,7 @@
aos::Sender<frc971::control_loops::drivetrain::CANPosition>
can_position_sender_;
- std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_,
- roller_falcon_, pivot_falcon_;
-
- std::optional<frc971::control_loops::CANFalconT> roller_falcon_data_;
-
- aos::stl_mutex roller_mutex_;
+ std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_;
// Pointer to the timer handler used to modify the wakeup.
::aos::TimerHandler *timer_handler_;
@@ -452,32 +360,8 @@
{
auto builder = superstructure_position_sender_.MakeBuilder();
- flatbuffers::Offset<frc971::control_loops::CANFalcon>
- roller_falcon_offset;
- frc971::PotAndAbsolutePositionT pivot;
- CopyPosition(pivot_encoder_, &pivot,
- Values::kPivotJointEncoderCountsPerRevolution(),
- Values::kPivotJointEncoderRatio(), pivot_pot_translate, true,
- values_->pivot_joint.potentiometer_offset);
-
- auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
- if (optional_roller_falcon.has_value()) {
- roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
- *builder.fbb(), &optional_roller_falcon.value());
- }
-
- flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_offset =
- frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &pivot);
-
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
- position_builder.add_end_effector_cube_beam_break(
- !end_effector_cube_beam_break_->Get());
- position_builder.add_pivot_joint_position(pivot_offset);
-
- if (!roller_falcon_offset.IsNull()) {
- position_builder.add_roller_falcon(roller_falcon_offset);
- }
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -556,26 +440,6 @@
superstructure_reading_ = superstructure_reading;
}
- void set_end_effector_cube_beam_break(
- ::std::unique_ptr<frc::DigitalInput> sensor) {
- end_effector_cube_beam_break_ = ::std::move(sensor);
- }
-
- void set_pivot_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- pivot_encoder_.set_encoder(::std::move(encoder));
- }
-
- void set_pivot_absolute_pwm(
- ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
- pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
- }
-
- void set_pivot_potentiometer(
- ::std::unique_ptr<frc::AnalogInput> potentiometer) {
- pivot_encoder_.set_potentiometer(::std::move(potentiometer));
- }
-
private:
std::shared_ptr<const Values> values_;
@@ -587,95 +451,12 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_,
- end_effector_cube_beam_break_;
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
- frc971::wpilib::AbsoluteEncoderAndPotentiometer pivot_encoder_;
-
CANSensorReader *can_sensor_reader_;
};
-
-class SuperstructureCANWriter
- : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
- public:
- SuperstructureCANWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
- event_loop, "/superstructure") {
- event_loop->SetRuntimeRealtimePriority(
- constants::Values::kSuperstructureCANWriterPriority);
-
- event_loop->OnRun([this]() { WriteConfigs(); });
- };
-
- void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
- roller_falcon_->PrintConfigs();
- pivot_falcon_->PrintConfigs();
- if (configuration.reapply()) {
- WriteConfigs();
- }
- }
-
- void set_roller_falcon(std::shared_ptr<Falcon> roller_falcon) {
- roller_falcon_ = std::move(roller_falcon);
- }
-
- void set_pivot_falcon(std::shared_ptr<Falcon> pivot_falcon) {
- pivot_falcon_ = std::move(pivot_falcon);
- }
-
- private:
- void WriteConfigs() {
- roller_falcon_->WriteRollerConfigs();
- pivot_falcon_->WritePivotConfigs();
- }
-
- void Write(const superstructure::Output &output) override {
- ctre::phoenix6::controls::DutyCycleOut roller_control(
- SafeSpeed(-output.roller_voltage()));
- roller_control.UpdateFreqHz = 0_Hz;
- roller_control.EnableFOC = true;
-
- ctre::phoenix6::controls::DutyCycleOut pivot_control(
- SafeSpeed(output.pivot_joint_voltage()));
- pivot_control.UpdateFreqHz = 0_Hz;
- pivot_control.EnableFOC = true;
-
- ctre::phoenix::StatusCode status =
- roller_falcon_->talon()->SetControl(roller_control);
-
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- status = pivot_falcon_->talon()->SetControl(pivot_control);
-
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
- status.GetName(), status.GetDescription());
- }
- }
-
- void Stop() override {
- AOS_LOG(WARNING, "Superstructure CAN output too old.\n");
- ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
- stop_command.UpdateFreqHz = 0_Hz;
- stop_command.EnableFOC = true;
-
- roller_falcon_->talon()->SetControl(stop_command);
- pivot_falcon_->talon()->SetControl(stop_command);
- }
-
- double SafeSpeed(double voltage) {
- return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
- }
-
- std::shared_ptr<Falcon> roller_falcon_;
- std::shared_ptr<Falcon> pivot_falcon_;
-};
-
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
::frc971::control_loops::drivetrain::Output> {
public:
@@ -817,10 +598,6 @@
std::make_shared<Falcon>(2, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> left_back =
std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> roller =
- std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> pivot =
- std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
// Thread 3.
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
@@ -829,7 +606,7 @@
std::move(signals_registry));
can_sensor_reader.set_falcons(right_front, right_back, left_front,
- left_back, roller, pivot);
+ left_back);
AddLoop(&can_sensor_reader_event_loop);
@@ -843,13 +620,6 @@
sensor_reader.set_superstructure_reading(superstructure_reading);
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(3));
- sensor_reader.set_end_effector_cube_beam_break(
- make_unique<frc::DigitalInput>(22));
-
- sensor_reader.set_pivot_encoder(make_encoder(0));
- sensor_reader.set_pivot_absolute_pwm(make_unique<frc::DigitalInput>(0));
- sensor_reader.set_pivot_potentiometer(make_unique<frc::AnalogInput>(0));
-
AddLoop(&sensor_reader_event_loop);
// Thread 5.
@@ -875,15 +645,10 @@
drivetrain_writer.set_left_inverted(
ctre::phoenix6::signals::InvertedValue::Clockwise_Positive);
- SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
- superstructure_can_writer.set_roller_falcon(roller);
- superstructure_can_writer.set_pivot_falcon(pivot);
-
can_output_event_loop.MakeWatcher(
- "/roborio", [&drivetrain_writer, &superstructure_can_writer](
- const frc971::CANConfiguration &configuration) {
+ "/roborio",
+ [&drivetrain_writer](const frc971::CANConfiguration &configuration) {
drivetrain_writer.HandleCANConfiguration(configuration);
- superstructure_can_writer.HandleCANConfiguration(configuration);
});
AddLoop(&can_output_event_loop);