Switch back to using one transfer roller voltage
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I42bdfec3813f009aa89066d4682658db956a5a89
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 266ebfa..3d5284e 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -70,8 +70,7 @@
const CatapultGoal *catapult_goal = nullptr;
double roller_speed_compensated_front = 0.0;
double roller_speed_compensated_back = 0.0;
- double transfer_roller_speed_front = 0.0;
- double transfer_roller_speed_back = 0.0;
+ double transfer_roller_speed = 0.0;
double flipper_arms_voltage = 0.0;
bool have_active_intake_request = false;
@@ -84,8 +83,7 @@
unsafe_goal->roller_speed_back() -
std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
- transfer_roller_speed_front = unsafe_goal->transfer_roller_speed_front();
- transfer_roller_speed_back = unsafe_goal->transfer_roller_speed_back();
+ transfer_roller_speed = unsafe_goal->transfer_roller_speed();
turret_goal =
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
@@ -175,13 +173,13 @@
}
// Check if we're either spitting of have lost the ball.
- if (transfer_roller_speed_front < 0.0 ||
+ if ((transfer_roller_speed < 0.0 && front_intake_has_ball_) ||
timestamp >
front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
front_intake_has_ball_ = false;
}
- if (transfer_roller_speed_back < 0.0 ||
+ if ((transfer_roller_speed > 0.0 && back_intake_has_ball_) ||
timestamp >
back_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
back_intake_has_ball_ = false;
@@ -194,18 +192,18 @@
if (front_intake_has_ball_) {
roller_speed_compensated_front = 0.0;
if (position->intake_beambreak_front()) {
- transfer_roller_speed_front = -wiggle_voltage;
+ transfer_roller_speed = -wiggle_voltage;
} else {
- transfer_roller_speed_front = wiggle_voltage;
+ transfer_roller_speed = wiggle_voltage;
}
}
if (back_intake_has_ball_) {
roller_speed_compensated_back = 0.0;
if (position->intake_beambreak_back()) {
- transfer_roller_speed_back = -wiggle_voltage;
+ transfer_roller_speed = wiggle_voltage;
} else {
- transfer_roller_speed_back = wiggle_voltage;
+ transfer_roller_speed = -wiggle_voltage;
}
}
@@ -296,11 +294,9 @@
// Transfer rollers and flipper arm belt on
if (turret_intake_state_ == RequestedIntake::kFront) {
- transfer_roller_speed_front =
- constants::Values::kTransferRollerVoltage();
+ transfer_roller_speed = constants::Values::kTransferRollerVoltage();
} else {
- transfer_roller_speed_back =
- constants::Values::kTransferRollerVoltage();
+ transfer_roller_speed = -constants::Values::kTransferRollerVoltage();
}
flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
@@ -495,8 +491,7 @@
if (output != nullptr) {
output_struct.roller_voltage_front = roller_speed_compensated_front;
output_struct.roller_voltage_back = roller_speed_compensated_back;
- output_struct.transfer_roller_voltage_front = transfer_roller_speed_front;
- output_struct.transfer_roller_voltage_back = transfer_roller_speed_back;
+ output_struct.transfer_roller_voltage = transfer_roller_speed;
output_struct.flipper_arms_voltage = flipper_arms_voltage;
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 7227dc2..0bb51e1 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -37,8 +37,10 @@
roller_speed_front:double (id: 3);
roller_speed_back:double (id: 4);
- transfer_roller_speed_front:double (id: 5);
- transfer_roller_speed_back:double (id: 12);
+ // Positive is intaking front and spitting back, negative is the opposite
+ transfer_roller_speed:double (id: 5);
+ // Not used anymore - just one transfer roller speed to control both
+ transfer_roller_speed_back:double (id: 12, deprecated);
// Factor to multiply robot velocity by and add to roller voltage.
roller_speed_compensation:double (id: 6);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 341d315..02fef22 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -778,10 +778,7 @@
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
- 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
@@ -822,10 +819,8 @@
IntakeState::INTAKE_FRONT_BALL);
EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
constants::Values::kFlipperFeedVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
constants::Values::kTransferRollerVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- 0.0);
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
@@ -843,10 +838,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
- 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -865,10 +857,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
- 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -884,10 +873,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
- 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -908,14 +894,12 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
- EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() <=
+ superstructure_output_fetcher_->transfer_roller_voltage() <=
constants::Values::kTransferRollerWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() >=
+ superstructure_output_fetcher_->transfer_roller_voltage() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(0.0,
- superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -963,14 +947,12 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
- EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() <=
+ superstructure_output_fetcher_->transfer_roller_voltage() <=
constants::Values::kTransferRollerWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() >=
+ superstructure_output_fetcher_->transfer_roller_voltage() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(0.0,
- superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::SHOOTING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -1212,8 +1194,7 @@
RunFor(chrono::milliseconds(2000));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_status_fetcher_->state(),
- SuperstructureState::IDLE);
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
}
// Test that we are able to signal that the ball was preloaded
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index af6c292..a4460e2 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -26,8 +26,9 @@
// positive is pulling into the robot
roller_voltage_front:double (id: 6);
roller_voltage_back:double (id: 7);
- transfer_roller_voltage_front:double (id: 8);
- transfer_roller_voltage_back:double (id: 9);
+ transfer_roller_voltage:double (id: 8);
+ // Only using one transfer roller voltage now
+ transfer_roller_voltage_back:double (id: 9, deprecated);
}
root_type Output;