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/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;