Merge "A few more updates to handle pi/orin naming"
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index d939743..2e89bfa 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -43,6 +43,12 @@
"intake_pivot_supply_current_limit": 35,
"intake_pivot_stator_current_limit": 60,
"intake_roller_supply_current_limit": 35,
- "intake_roller_stator_current_limit": 60
+ "intake_roller_stator_current_limit": 60,
+ "transfer_roller_supply_current_limit": 35,
+ "transfer_roller_stator_current_limit": 60
+ },
+ "transfer_roller_voltages": {
+ "transfer_in": 12.0,
+ "transfer_out": -12.0
}
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index e648df0..54e6925 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -48,6 +48,13 @@
intake_pivot_stator_current_limit:double (id: 1);
intake_roller_supply_current_limit:double (id: 2);
intake_roller_stator_current_limit:double (id: 3);
+ transfer_roller_supply_current_limit:double (id: 4);
+ transfer_roller_stator_current_limit:double (id: 5);
+}
+
+table TransferRollerVoltages {
+ transfer_in:double (id: 0);
+ transfer_out:double (id: 1);
}
table RobotConstants {
@@ -63,6 +70,7 @@
intake_pivot:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 4);
drivetrain:frc971.control_loops.drivetrain.fbs.DrivetrainConfig (id: 5);
current_limits:CurrentLimits (id: 6);
+ transfer_roller_voltages:TransferRollerVoltages (id: 7);
}
table Constants {
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 366a8db..9a703e3 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -30,6 +30,7 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ transfer_goal_(TransferRollerGoal::NONE),
intake_pivot_(
robot_constants_->common()->intake_pivot(),
robot_constants_->robot()->intake_constants()->intake_pivot_zero()) {
@@ -70,17 +71,44 @@
output_struct.intake_roller_voltage = 0.0;
break;
case IntakeRollerGoal::SPIT:
+ transfer_goal_ = TransferRollerGoal::TRANSFER_OUT;
intake_roller_state = IntakeRollerState::SPITTING;
output_struct.intake_roller_voltage =
robot_constants_->common()->intake_roller_voltages()->spitting();
break;
case IntakeRollerGoal::INTAKE:
+ transfer_goal_ = TransferRollerGoal::TRANSFER_IN;
intake_roller_state = IntakeRollerState::INTAKING;
output_struct.intake_roller_voltage =
robot_constants_->common()->intake_roller_voltages()->intaking();
break;
}
+ TransferRollerState transfer_roller_state = TransferRollerState::NONE;
+
+ switch (unsafe_goal != nullptr ? transfer_goal_ : TransferRollerGoal::NONE) {
+ case TransferRollerGoal::NONE:
+ output_struct.transfer_roller_voltage = 0.0;
+ break;
+ case TransferRollerGoal::TRANSFER_IN:
+ if (position->transfer_beambreak()) {
+ transfer_goal_ = TransferRollerGoal::NONE;
+ transfer_roller_state = TransferRollerState::NONE;
+ output_struct.transfer_roller_voltage = 0.0;
+ break;
+ }
+ transfer_roller_state = TransferRollerState::TRANSFERING_IN;
+ output_struct.transfer_roller_voltage =
+ robot_constants_->common()->transfer_roller_voltages()->transfer_in();
+ break;
+ case TransferRollerGoal::TRANSFER_OUT:
+ transfer_roller_state = TransferRollerState::TRANSFERING_OUT;
+ output_struct.transfer_roller_voltage = robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_out();
+ break;
+ }
+
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
alliance_ = joystick_state_fetcher_->alliance();
@@ -118,6 +146,7 @@
status_builder.add_estopped(estopped);
status_builder.add_intake_roller_state(intake_roller_state);
status_builder.add_intake_pivot_state(intake_pivot_status_offset);
+ status_builder.add_transfer_roller_state(transfer_roller_state);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 277ba73..67a8328 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -51,6 +51,7 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ TransferRollerGoal transfer_goal_;
PotAndAbsoluteEncoderSubsystem intake_pivot_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2024/control_loops/superstructure/superstructure_can_position.fbs b/y2024/control_loops/superstructure/superstructure_can_position.fbs
index 46f638c..f9f4ca9 100644
--- a/y2024/control_loops/superstructure/superstructure_can_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_can_position.fbs
@@ -17,6 +17,9 @@
// CAN Position of the intake pivot falcon
intake_pivot:frc971.control_loops.CANTalonFX (id: 3);
+
+ // CAN Position of the transfer roller falcon
+ transfer_roller:frc971.control_loops.CANTalonFX (id: 4);
}
root_type CANPosition;
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index 63cbcf0..0af8ca3 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -16,11 +16,22 @@
RETRACTED = 1,
}
+// Represents goal of transfer rollers
+// TRANSFER_IN is for transfering game piece in from the intake to the shooter
+// TRANSFER_OUT is for transfering game piece out to the intake for spitting
+enum TransferRollerGoal : ubyte {
+ NONE = 0,
+ TRANSFER_IN = 1,
+ TRANSFER_OUT = 2,
+}
+
table Goal {
intake_roller_goal:IntakeRollerGoal (id: 0);
intake_pivot_goal:IntakePivotGoal (id: 1);
catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 2);
+
+ transfer_roller_goal:TransferRollerGoal (id: 3);
}
root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index c253f62..26e75e4 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -51,6 +51,7 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
+ transfer_beambreak_(false),
intake_pivot_(
new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
PositionSensorSimulator(simulated_robot_constants->robot()
@@ -105,12 +106,17 @@
Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_transfer_beambreak(transfer_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
+ void set_transfer_beambreak(bool triggered) {
+ transfer_beambreak_ = triggered;
+ }
+
PotAndAbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
private:
@@ -122,6 +128,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
+ bool transfer_beambreak_;
+
PotAndAbsoluteEncoderSimulator intake_pivot_;
bool first_ = true;
};
@@ -375,7 +383,7 @@
CheckIfZeroed();
}
-// Tests Intake in multiple scenarios
+// Tests intake and transfer in multiple scenarios
TEST_F(SuperstructureTest, IntakeGoal) {
SetEnabled(true);
WaitUntilZeroed();
@@ -398,6 +406,8 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_transfer_beambreak(false);
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -415,10 +425,17 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_transfer_beambreak(false);
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ simulated_robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_out());
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -430,8 +447,39 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_transfer_beambreak(false);
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_transfer_beambreak(false);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ simulated_robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_in());
+
+ superstructure_plant_.set_transfer_beambreak(true);
+
+ RunFor(chrono::seconds(2));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
}
} // namespace y2024::control_loops::superstructure::testing
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/superstructure_output.fbs b/y2024/control_loops/superstructure/superstructure_output.fbs
index cd5971e..976707d 100644
--- a/y2024/control_loops/superstructure/superstructure_output.fbs
+++ b/y2024/control_loops/superstructure/superstructure_output.fbs
@@ -15,6 +15,11 @@
// Voltage of the catapult
catapult_voltage: double (id: 4);
+
+ // Voltage of transfer rollers
+ // Positive voltage is for transfering in
+ // Negative voltage is for transfering out
+ transfer_roller_voltage:double (id: 5);
}
root_type Output;
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
index e4f6474..da867a9 100644
--- a/y2024/control_loops/superstructure/superstructure_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -15,6 +15,9 @@
// Values of the encoder and potentiometer at the catapult
catapult:frc971.PotAndAbsolutePosition (id: 3);
+
+ // True means there is a game piece in the transfer.
+ transfer_beambreak:bool (id: 4);
}
root_type Position;
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 82756dd..9084361 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -21,6 +21,13 @@
altitude_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
}
+// Contains status of transfer rollers
+enum TransferRollerState : ubyte {
+ NONE = 0,
+ TRANSFERING_IN = 1,
+ TRANSFERING_OUT = 2,
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -33,6 +40,9 @@
// Estimated angle and angular velocitiy of the intake.
intake_pivot_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 3);
+
+ // State of transfer rollers
+ transfer_roller_state:TransferRollerState (id: 4);
}
root_type Status;
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 2217513..b708266 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -140,6 +140,7 @@
->intake_constants()
->potentiometer_offset());
+ builder->set_transfer_beambreak(transfer_beam_break_->Get());
builder.CheckOk(builder.Send());
}
@@ -212,6 +213,10 @@
intake_pivot_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ void set_transfer_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
+ transfer_beam_break_ = ::std::move(sensor);
+ }
+
private:
const Constants *robot_constants_;
@@ -223,7 +228,7 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_, transfer_beam_break_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_pivot_encoder_;
@@ -272,6 +277,7 @@
make_unique<frc::DigitalInput>(4));
sensor_reader.set_intake_pivot_potentiometer(
make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_transfer_beambreak(make_unique<frc::DigitalInput>(7));
AddLoop(&sensor_reader_event_loop);
@@ -316,7 +322,14 @@
robot_constants->common()
->current_limits()
->intake_roller_supply_current_limit());
-
+ std::shared_ptr<TalonFX> transfer_roller =
+ std::make_shared<TalonFX>(6, false, "Drivetrain Bus", &signals_registry,
+ robot_constants->common()
+ ->current_limits()
+ ->transfer_roller_stator_current_limit(),
+ robot_constants->common()
+ ->current_limits()
+ ->transfer_roller_supply_current_limit());
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
@@ -334,7 +347,7 @@
talonfxs.push_back(talonfx);
}
- for (auto talonfx : {intake_pivot, intake_roller}) {
+ for (auto talonfx : {intake_pivot, intake_roller, transfer_roller}) {
talonfxs.push_back(talonfx);
}
@@ -352,7 +365,7 @@
frc971::wpilib::CANSensorReader can_sensor_reader(
&can_sensor_reader_event_loop, std::move(signals_registry), talonfxs,
- [drivetrain_talonfxs, &intake_pivot, &intake_roller,
+ [drivetrain_talonfxs, &intake_pivot, &intake_roller, &transfer_roller,
&drivetrain_can_position_sender, &superstructure_can_position_sender](
ctre::phoenix::StatusCode status) {
aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
@@ -384,6 +397,9 @@
intake_pivot->SerializePosition(
superstructure_can_builder->add_intake_pivot(),
control_loops::drivetrain::kHighOutputRatio);
+ transfer_roller->SerializePosition(
+ superstructure_can_builder->add_transfer_roller(),
+ control_loops::drivetrain::kHighOutputRatio);
superstructure_can_builder->set_timestamp(
intake_roller->GetTimestamp());
@@ -410,6 +426,8 @@
->second->WriteVoltage(output.intake_pivot_voltage());
talonfx_map.find("intake_roller")
->second->WriteVoltage(output.intake_roller_voltage());
+ talonfx_map.find("transfer_roller")
+ ->second->WriteVoltage(output.transfer_roller_voltage());
});
can_drivetrain_writer.set_talonfxs({right_front, right_back},
@@ -417,6 +435,7 @@
can_superstructure_writer.add_talonfx("intake_pivot", intake_pivot);
can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
+ can_superstructure_writer.add_talonfx("transfer_roller", transfer_roller);
can_output_event_loop.MakeWatcher(
"/roborio", [&can_drivetrain_writer, &can_superstructure_writer](