Switch back to using one transfer roller voltage
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I42bdfec3813f009aa89066d4682658db956a5a89
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index ecc2a98..a8a2905 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -351,10 +351,7 @@
if (requested_intake_.has_value()) {
superstructure_builder.add_turret_intake(*requested_intake_);
}
- superstructure_builder.add_transfer_roller_speed_front(
- transfer_roller_front_voltage_);
- superstructure_builder.add_transfer_roller_speed_back(
- transfer_roller_back_voltage_);
+ superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
superstructure_builder.add_turret(turret_offset);
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
@@ -371,8 +368,7 @@
set_requested_intake(RequestedIntake::kFront);
set_intake_front_goal(kExtendIntakeGoal);
set_roller_front_voltage(kIntakeRollerVoltage);
- set_transfer_roller_front_voltage(kRollerVoltage);
- set_transfer_roller_back_voltage(-kRollerVoltage);
+ set_transfer_roller_voltage(kRollerVoltage);
SendSuperstructureGoal();
}
@@ -380,8 +376,7 @@
set_requested_intake(std::nullopt);
set_intake_front_goal(kRetractIntakeGoal);
set_roller_front_voltage(0.0);
- set_transfer_roller_front_voltage(0.0);
- set_transfer_roller_back_voltage(0.0);
+ set_transfer_roller_voltage(0.0);
SendSuperstructureGoal();
}
@@ -389,8 +384,7 @@
set_requested_intake(RequestedIntake::kBack);
set_intake_back_goal(kExtendIntakeGoal);
set_roller_back_voltage(kIntakeRollerVoltage);
- set_transfer_roller_back_voltage(kRollerVoltage);
- set_transfer_roller_front_voltage(-kRollerVoltage);
+ set_transfer_roller_voltage(-kRollerVoltage);
SendSuperstructureGoal();
}
@@ -398,8 +392,7 @@
set_requested_intake(std::nullopt);
set_intake_back_goal(kRetractIntakeGoal);
set_roller_back_voltage(0.0);
- set_transfer_roller_front_voltage(0.0);
- set_transfer_roller_back_voltage(0.0);
+ set_transfer_roller_voltage(0.0);
SendSuperstructureGoal();
}
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 0168630..ec66fb3 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -41,11 +41,8 @@
void set_roller_back_voltage(double roller_back_voltage) {
roller_back_voltage_ = roller_back_voltage;
}
- void set_transfer_roller_front_voltage(double voltage) {
- transfer_roller_front_voltage_ = voltage;
- }
- void set_transfer_roller_back_voltage(double voltage) {
- transfer_roller_back_voltage_ = voltage;
+ void set_transfer_roller_voltage(double voltage) {
+ transfer_roller_voltage_ = voltage;
}
void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
requested_intake_ = requested_intake;
@@ -78,8 +75,7 @@
double intake_back_goal_ = 0.0;
double roller_front_voltage_ = 0.0;
double roller_back_voltage_ = 0.0;
- double transfer_roller_front_voltage_ = 0.0;
- double transfer_roller_back_voltage_ = 0.0;
+ double transfer_roller_voltage_ = 0.0;
std::optional<RequestedIntake> requested_intake_ = std::nullopt;
double turret_goal_ = 0.0;
bool fire_ = false;
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;
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index cfd8952..7e7a735 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -45,7 +45,8 @@
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kSpit(3, 3);
+const ButtonLocation kSpitFront(3, 3);
+const ButtonLocation kSpitBack(2, 3);
const ButtonLocation kRedLocalizerReset(3, 13);
const ButtonLocation kBlueLocalizerReset(3, 14);
@@ -62,7 +63,8 @@
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kSpit(3, 3);
+const ButtonLocation kSpitFront(3, 3);
+const ButtonLocation kSpitBack(2, 3);
const ButtonLocation kRedLocalizerReset(4, 14);
const ButtonLocation kBlueLocalizerReset(4, 13);
@@ -165,8 +167,7 @@
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;
+ double transfer_roller_speed = 0.0;
std::optional<control_loops::superstructure::RequestedIntake>
requested_intake;
@@ -228,21 +229,21 @@
// Extend the intakes and spin the rollers
if (data.IsPressed(kIntakeFrontOut)) {
intake_front_pos = kIntakePosition;
- transfer_roller_front_speed = kTransferRollerSpeed;
+ transfer_roller_speed = kTransferRollerSpeed;
intake_front_counter_ = kIntakeCounterIterations;
intake_back_counter_ = 0;
} else if (data.IsPressed(kIntakeBackOut)) {
intake_back_pos = kIntakePosition;
- transfer_roller_back_speed = kTransferRollerSpeed;
+ transfer_roller_speed = -kTransferRollerSpeed;
intake_back_counter_ = kIntakeCounterIterations;
intake_front_counter_ = 0;
- } else if (data.IsPressed(kSpit)) {
- transfer_roller_front_speed = -kTransferRollerSpeed;
- transfer_roller_back_speed = -kTransferRollerSpeed;
-
+ } else if (data.IsPressed(kSpitFront)) {
+ transfer_roller_speed = -kTransferRollerSpeed;
intake_front_counter_ = 0;
+ } else if (data.IsPressed(kSpitBack)) {
+ transfer_roller_speed = kTransferRollerSpeed;
intake_back_counter_ = 0;
}
@@ -323,10 +324,8 @@
superstructure_goal_builder.add_roller_speed_front(roller_front_speed);
superstructure_goal_builder.add_roller_speed_back(roller_back_speed);
- superstructure_goal_builder.add_transfer_roller_speed_front(
- transfer_roller_front_speed);
- superstructure_goal_builder.add_transfer_roller_speed_back(
- transfer_roller_back_speed);
+ superstructure_goal_builder.add_transfer_roller_speed(
+ transfer_roller_speed);
superstructure_goal_builder.add_auto_aim(data.IsPressed(kAutoAim));
if (requested_intake.has_value()) {
superstructure_goal_builder.add_turret_intake(requested_intake.value());
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 3a03a2a..56eba95 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -437,7 +437,6 @@
::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
catapult_falcon_1_can_, catapult_falcon_2_can_;
-
};
class SuperstructureWriter
@@ -473,9 +472,12 @@
falcon->ConfigStatorCurrentLimit(
{false, Values::kIntakeRollerStatorCurrentLimit(),
Values::kIntakeRollerStatorCurrentLimit(), 0});
- falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_1_General, 1);
- falcon->SetControlFramePeriod(ctre::phoenix::motorcontrol::Control_3_General, 1);
- falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
+ falcon->SetStatusFramePeriod(
+ ctre::phoenix::motorcontrol::Status_1_General, 1);
+ falcon->SetControlFramePeriod(
+ ctre::phoenix::motorcontrol::Control_3_General, 1);
+ falcon->SetStatusFramePeriod(
+ ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
falcon->ConfigOpenloopRamp(0.0);
falcon->ConfigClosedloopRamp(0.0);
falcon->ConfigVoltageMeasurementFilter(1);
@@ -528,12 +530,8 @@
return flipper_arms_falcon_;
}
- void set_transfer_roller_victor_front(::std::unique_ptr<::frc::VictorSP> t) {
- transfer_roller_victor_front_ = ::std::move(t);
- }
-
- void set_transfer_roller_victor_back(::std::unique_ptr<::frc::VictorSP> t) {
- transfer_roller_victor_back_ = ::std::move(t);
+ void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ transfer_roller_victor_ = ::std::move(t);
}
std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
@@ -555,8 +553,7 @@
ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
intake_falcon_front_->SetDisabled();
intake_falcon_back_->SetDisabled();
- transfer_roller_victor_front_->SetDisabled();
- transfer_roller_victor_back_->SetDisabled();
+ transfer_roller_victor_->SetDisabled();
if (catapult_falcon_1_) {
catapult_falcon_1_->SetDisabled();
}
@@ -576,10 +573,7 @@
WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
WriteCan(output.roller_voltage_front(), roller_falcon_front_.get());
WriteCan(output.roller_voltage_back(), roller_falcon_back_.get());
- WritePwm(output.transfer_roller_voltage_front(),
- transfer_roller_victor_front_.get());
- WritePwm(-output.transfer_roller_voltage_back(),
- transfer_roller_victor_back_.get());
+ WritePwm(output.transfer_roller_voltage(), transfer_roller_victor_.get());
WriteCan(-output.flipper_arms_voltage(), flipper_arms_falcon_.get());
@@ -626,8 +620,7 @@
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
climber_falcon_;
- ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_front_,
- transfer_roller_victor_back_;
+ ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
std::unique_ptr<frc::DigitalOutput> catapult_reversal_;
};
@@ -768,9 +761,7 @@
superstructure_writer.set_roller_falcon_back(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
- superstructure_writer.set_transfer_roller_victor_front(
- make_unique<::frc::VictorSP>(6));
- superstructure_writer.set_transfer_roller_victor_back(
+ superstructure_writer.set_transfer_roller_victor(
make_unique<::frc::VictorSP>(5));
superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(2));