Add second transfer roller motor
Still use same behavior of always sending opposite voltages
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: If6bcf617db37b40924556d59dcfa23e7d43bdef1
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b25d074..a5b4df9 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -66,7 +66,8 @@
*turret_goal = nullptr;
double roller_speed_compensated_front = 0.0;
double roller_speed_compensated_back = 0.0;
- double transfer_roller_speed = 0.0;
+ double transfer_roller_speed_front = 0.0;
+ double transfer_roller_speed_back = 0.0;
double flipper_arms_voltage = 0.0;
if (unsafe_goal != nullptr) {
@@ -78,7 +79,8 @@
unsafe_goal->roller_speed_back() -
std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
- transfer_roller_speed = unsafe_goal->transfer_roller_speed();
+ transfer_roller_speed_front = unsafe_goal->transfer_roller_speed_front();
+ transfer_roller_speed_back = unsafe_goal->transfer_roller_speed_back();
turret_goal =
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
@@ -112,9 +114,9 @@
// to IDLE.
const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
- transfer_roller_speed < 0) ||
+ transfer_roller_speed_front < 0) ||
(intake_state_ == IntakeState::INTAKE_BACK_BALL &&
- transfer_roller_speed > 0));
+ transfer_roller_speed_back < 0));
// Intake handling should happen regardless of the turret state
if (position->intake_beambreak_front() || position->intake_beambreak_back()) {
@@ -135,23 +137,29 @@
}
if (intake_state_ != IntakeState::NO_BALL) {
- // Block intaking in
roller_speed_compensated_front = 0.0;
roller_speed_compensated_back = 0.0;
const double wiggle_voltage =
- (intake_state_ == IntakeState::INTAKE_FRONT_BALL
- ? constants::Values::kTransferRollerFrontWiggleVoltage()
- : constants::Values::kTransferRollerBackWiggleVoltage());
+ constants::Values::kTransferRollerWiggleVoltage();
// Wiggle transfer rollers: send the ball back and forth while waiting
// for the turret or waiting for another shot to be completed
- if ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
- position->intake_beambreak_front()) ||
- (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
- position->intake_beambreak_back())) {
- transfer_roller_speed = -wiggle_voltage;
+ if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
+ if (position->intake_beambreak_front()) {
+ transfer_roller_speed_front = -wiggle_voltage;
+ transfer_roller_speed_back = wiggle_voltage;
+ } else {
+ transfer_roller_speed_front = wiggle_voltage;
+ transfer_roller_speed_back = -wiggle_voltage;
+ }
} else {
- transfer_roller_speed = wiggle_voltage;
+ if (position->intake_beambreak_back()) {
+ transfer_roller_speed_back = -wiggle_voltage;
+ transfer_roller_speed_front = wiggle_voltage;
+ } else {
+ transfer_roller_speed_back = wiggle_voltage;
+ transfer_roller_speed_front = -wiggle_voltage;
+ }
}
}
@@ -198,10 +206,17 @@
}
// Transfer rollers and flipper arm belt on
- transfer_roller_speed =
- (intake_state_ == IntakeState::INTAKE_FRONT_BALL
- ? constants::Values::kTransferRollerFrontVoltage()
- : constants::Values::kTransferRollerBackVoltage());
+ if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
+ transfer_roller_speed_front =
+ constants::Values::kTransferRollerVoltage();
+ transfer_roller_speed_back =
+ -constants::Values::kTransferRollerVoltage();
+ } else {
+ transfer_roller_speed_back =
+ constants::Values::kTransferRollerVoltage();
+ transfer_roller_speed_front =
+ -constants::Values::kTransferRollerVoltage();
+ }
flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
// Ball is in catapult
@@ -373,7 +388,8 @@
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 = transfer_roller_speed;
+ output_struct.transfer_roller_voltage_front = transfer_roller_speed_front;
+ output_struct.transfer_roller_voltage_back = transfer_roller_speed_back;
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 c86f338..6266b3c 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -28,8 +28,8 @@
// Positive is rollers intaking.
roller_speed_front:double (id: 3);
roller_speed_back:double (id: 4);
- // One transfer motor for both sides
- transfer_roller_speed:double (id: 5);
+ transfer_roller_speed_front:double (id: 5);
+ transfer_roller_speed_back:double (id: 12);
// 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 2f73370..af2ba53 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -473,20 +473,6 @@
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), expected);
}
- void TestTransferRoller(double transfer_roller_speed,
- double roller_speed_compensation, double expected) {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_transfer_roller_speed(transfer_roller_speed);
- goal_builder.add_roller_speed_compensation(roller_speed_compensation);
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- RunFor(dt() * 2);
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
- expected);
- }
-
std::shared_ptr<const constants::Values> values_;
const aos::Node *const roborio_;
@@ -760,10 +746,6 @@
TestRollerBack(-12.0, 1.5, -7.5);
TestRollerBack(12.0, 1.5, 16.5);
TestRollerBack(0.0, 1.5, 4.5);
-
- TestTransferRoller(-12.0, 1.5, -12.0);
- TestTransferRoller(12.0, 1.5, 12.0);
- TestTransferRoller(0.0, 1.5, 0.0);
}
// Tests the whole shooting statemachine - from loading to shooting
@@ -794,7 +776,10 @@
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(), 0.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_status_fetcher_->state(), SuperstructureState::IDLE);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
@@ -833,8 +818,10 @@
IntakeState::INTAKE_FRONT_BALL);
EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
constants::Values::kFlipperFeedVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
- constants::Values::kTransferRollerFrontVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
+ constants::Values::kTransferRollerVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
+ -constants::Values::kTransferRollerVoltage());
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
@@ -852,7 +839,10 @@
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(), 0.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_status_fetcher_->state(),
SuperstructureState::LOADING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -871,7 +861,10 @@
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(), 0.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_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -887,7 +880,10 @@
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(), 0.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_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -908,13 +904,14 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
- LOG(INFO) << superstructure_output_fetcher_->transfer_roller_voltage();
- EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_front() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage() <=
- constants::Values::kTransferRollerFrontWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage() >=
- -constants::Values::kTransferRollerFrontWiggleVoltage());
+ superstructure_output_fetcher_->transfer_roller_voltage_front() <=
+ constants::Values::kTransferRollerWiggleVoltage() &&
+ superstructure_output_fetcher_->transfer_roller_voltage_front() >=
+ -constants::Values::kTransferRollerWiggleVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
+ -superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -960,12 +957,14 @@
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() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage() <=
- constants::Values::kTransferRollerFrontWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage() >=
- -constants::Values::kTransferRollerFrontWiggleVoltage());
+ superstructure_output_fetcher_->transfer_roller_voltage_back() <=
+ constants::Values::kTransferRollerWiggleVoltage() &&
+ superstructure_output_fetcher_->transfer_roller_voltage_back() >=
+ -constants::Values::kTransferRollerWiggleVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
+ -superstructure_output_fetcher_->transfer_roller_voltage_back());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::SHOOTING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index 8fa992e..af6c292 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -26,8 +26,8 @@
// positive is pulling into the robot
roller_voltage_front:double (id: 6);
roller_voltage_back:double (id: 7);
- // One transfer motor for both sides
- transfer_roller_voltage:double (id: 8);
+ transfer_roller_voltage_front:double (id: 8);
+ transfer_roller_voltage_back:double (id: 9);
}
root_type Output;