Merge "Set requested intake in autonomous"
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index fc02071..339d7bf 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -19,8 +19,9 @@
namespace y2022 {
namespace actors {
namespace {
-constexpr double kExtendIntakeGoal = 0.0;
+constexpr double kExtendIntakeGoal = -0.02;
constexpr double kRetractIntakeGoal = 1.47;
+constexpr double kIntakeRollerVoltage = 8.0;
constexpr double kRollerVoltage = 12.0;
constexpr double kCatapultReturnPosition = -0.908;
} // namespace
@@ -352,7 +353,7 @@
void AutonomousActor::ExtendFrontIntake() {
set_requested_intake(RequestedIntake::kFront);
set_intake_front_goal(kExtendIntakeGoal);
- set_roller_front_voltage(kRollerVoltage);
+ set_roller_front_voltage(kIntakeRollerVoltage);
set_transfer_roller_front_voltage(kRollerVoltage);
set_transfer_roller_back_voltage(-kRollerVoltage);
SendSuperstructureGoal();
@@ -361,7 +362,7 @@
void AutonomousActor::RetractFrontIntake() {
set_requested_intake(std::nullopt);
set_intake_front_goal(kRetractIntakeGoal);
- set_roller_front_voltage(kRollerVoltage);
+ set_roller_front_voltage(0.0);
set_transfer_roller_front_voltage(0.0);
set_transfer_roller_back_voltage(0.0);
SendSuperstructureGoal();
@@ -370,7 +371,7 @@
void AutonomousActor::ExtendBackIntake() {
set_requested_intake(RequestedIntake::kBack);
set_intake_back_goal(kExtendIntakeGoal);
- set_roller_back_voltage(kRollerVoltage);
+ set_roller_back_voltage(kIntakeRollerVoltage);
set_transfer_roller_back_voltage(kRollerVoltage);
set_transfer_roller_front_voltage(-kRollerVoltage);
SendSuperstructureGoal();
@@ -379,7 +380,7 @@
void AutonomousActor::RetractBackIntake() {
set_requested_intake(std::nullopt);
set_intake_back_goal(kRetractIntakeGoal);
- set_roller_back_voltage(kRollerVoltage);
+ set_roller_back_voltage(0.0);
set_transfer_roller_front_voltage(0.0);
set_transfer_roller_back_voltage(0.0);
SendSuperstructureGoal();
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 15201f7..edac6be 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -48,9 +48,9 @@
OutputT output_struct;
aos::FlatbufferFixedAllocatorArray<
- frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
turret_loading_goal_buffer;
- aos::FlatbufferFixedAllocatorArray<CatapultGoal, 64> catapult_goal_buffer;
+ aos::FlatbufferFixedAllocatorArray<CatapultGoal, 512> catapult_goal_buffer;
const aos::monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
@@ -96,16 +96,17 @@
const double distance_to_goal = aimer_.DistanceToGoal();
if (unsafe_goal->auto_aim() && values_->shot_interpolation_table.GetInRange(
distance_to_goal, &shot_params)) {
+ flatbuffers::FlatBufferBuilder *catapult_goal_fbb =
+ catapult_goal_buffer.fbb();
std::optional<flatbuffers::Offset<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>>
return_position_offset;
if (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
unsafe_goal->catapult()->has_return_position()) {
- return_position_offset = {
- aos::CopyFlatBuffer(unsafe_goal->catapult()->return_position(),
- catapult_goal_buffer.fbb())};
+ return_position_offset = {aos::CopyFlatBuffer(
+ unsafe_goal->catapult()->return_position(), catapult_goal_fbb)};
}
- CatapultGoal::Builder builder(*catapult_goal_buffer.fbb());
+ CatapultGoal::Builder builder(*catapult_goal_fbb);
builder.add_shot_position(shot_params.shot_angle);
builder.add_shot_velocity(shot_params.shot_velocity);
if (return_position_offset.has_value()) {
@@ -237,10 +238,10 @@
case SuperstructureState::IDLE: {
// Only change the turret's goal loading position when idle, to prevent us
// spinning the turret around when TRANSFERRING...
+ if (have_active_intake_request) {
+ turret_intake_state_ = unsafe_goal->turret_intake();
+ }
if (front_intake_has_ball_ != back_intake_has_ball_) {
- if (have_active_intake_request) {
- turret_intake_state_ = unsafe_goal->turret_intake();
- }
turret_intake_state_ = front_intake_has_ball_ ? RequestedIntake::kFront
: RequestedIntake::kBack;
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 08edbf0..6c19052 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -998,6 +998,8 @@
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+ constants::Values::kTurretBackIntakePos(), 0.001);
// Since the intake beambreak hasn't triggered in a while, it should realize
// the ball was lost.
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index a911e4b..a88e552 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -58,6 +58,7 @@
const ButtonLocation kAutoAim(4, 2);
const ButtonLocation kClimberExtend(4, 6);
+const ButtonLocation kClimberIntakes(4, 5);
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
@@ -161,8 +162,9 @@
setpoint_fetcher_.Fetch();
// Default to the intakes in
- double intake_front_pos = 1.47;
- double intake_back_pos = 1.47;
+ constexpr double kIntakeUpPosition = 1.47;
+ double intake_front_pos = kIntakeUpPosition;
+ double intake_back_pos = kIntakeUpPosition;
double transfer_roller_front_speed = 0.0;
double transfer_roller_back_speed = 0.0;
std::optional<control_loops::superstructure::RequestedIntake>
@@ -258,6 +260,13 @@
if (data.IsPressed(kFire)) {
fire = true;
+ // Provide a default turret goal.
+ turret_pos = 0.0;
+ }
+
+ if (data.IsPressed(kClimberIntakes)) {
+ intake_back_pos = kIntakeUpPosition;
+ intake_front_pos = kIntakePosition;
}
{
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 1971312..e75c014 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -882,6 +882,7 @@
event_loop_
->MakeFetcher<y2022::control_loops::superstructure::Status>(
"/superstructure")),
+ zeroer_(zeroing::ImuZeroer::FaultBehavior::kTemporary),
left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
event_loop_->MakeWatcher(
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index f23d3e2..b11dc3a 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -273,7 +273,8 @@
auto builder = gyro_sender_.MakeBuilder();
::frc971::sensors::GyroReading::Builder gyro_reading_builder =
builder.MakeBuilder<::frc971::sensors::GyroReading>();
- constexpr double kMaxVelocity = 2000; // degrees / second
+ // +/- 2000 deg / sec
+ constexpr double kMaxVelocity = 4000; // degrees / second
constexpr double kVelocityRadiansPerSecond =
kMaxVelocity / 360 * (2.0 * M_PI);