Merge "Allow skipping to superstructure loaded state"
diff --git a/frc971/input/joystick_state.fbs b/frc971/input/joystick_state.fbs
index 2f48bdf..49d2690 100644
--- a/frc971/input/joystick_state.fbs
+++ b/frc971/input/joystick_state.fbs
@@ -1,5 +1,7 @@
namespace aos;
+enum MatchType : byte { kNone, kPractice, kQualification, kElimination }
+
table Joystick {
// A bitmask of the butotn state.
buttons:ushort (id: 0);
@@ -44,6 +46,14 @@
// String corresponding to the game data string
game_data:string (id: 10);
+
+ // Driver station location.
+ location:ubyte (id: 11);
+
+ match_number:int (id: 12);
+ replay_number:int (id: 13);
+ match_type:MatchType (id: 14);
+ event_name:string (id: 15);
}
root_type JoystickState;
diff --git a/frc971/wpilib/ahal/DriverStation.cc b/frc971/wpilib/ahal/DriverStation.cc
index 2ed0a97..8b31af7 100644
--- a/frc971/wpilib/ahal/DriverStation.cc
+++ b/frc971/wpilib/ahal/DriverStation.cc
@@ -272,6 +272,54 @@
}
/**
+ * Returns the game specific message provided by the FMS.
+ *
+ * @return A string containing the game specific message.
+ */
+std::string_view DriverStation::GetGameSpecificMessage() const {
+ return std::string_view(
+ reinterpret_cast<const char *>(info_.gameSpecificMessage),
+ info_.gameSpecificMessageSize);
+}
+
+/**
+ * Returns the name of the competition event provided by the FMS.
+ *
+ * @return A string containing the event name
+ */
+std::string_view DriverStation::GetEventName() const {
+ return info_.eventName;
+}
+
+/**
+ * Returns the match number provided by the FMS.
+ *
+ * @return The number of the match
+ */
+DriverStation::MatchType DriverStation::GetMatchType() const {
+ return static_cast<DriverStation::MatchType>(info_.matchType);
+}
+
+/**
+ * Returns the match number provided by the FMS.
+ *
+ * @return The number of the match
+ */
+int DriverStation::GetMatchNumber() const {
+ return info_.matchNumber;
+}
+
+/**
+ * Returns the number of times the current match has been replayed from the
+ * FMS.
+ *
+ * @return The number of replays
+ */
+int DriverStation::GetReplayNumber() const {
+ return info_.replayNumber;
+}
+
+/**
* Return the alliance that the driver station says it is on.
*
* This could return kRed or kBlue.
@@ -374,10 +422,14 @@
HAL_ControlWord control_word;
HAL_GetControlWord(&control_word);
- is_enabled_ = control_word.enabled;
+ is_enabled_ = control_word.enabled && control_word.dsAttached;
is_autonomous_ = control_word.autonomous;
is_test_mode_ = control_word.test;
is_fms_attached_ = control_word.fmsAttached;
+ is_ds_attached_ = control_word.dsAttached;
+ is_teleop_ = !(control_word.autonomous || control_word.test);
+
+ HAL_GetMatchInfo(&info_);
}
/**
diff --git a/frc971/wpilib/ahal/DriverStation.h b/frc971/wpilib/ahal/DriverStation.h
index 5150360..8bcdcf1 100644
--- a/frc971/wpilib/ahal/DriverStation.h
+++ b/frc971/wpilib/ahal/DriverStation.h
@@ -27,6 +27,7 @@
class DriverStation {
public:
enum Alliance { kRed, kBlue, kInvalid };
+ enum MatchType { kNone, kPractice, kQualification, kElimination };
virtual ~DriverStation();
static DriverStation &GetInstance();
@@ -55,10 +56,18 @@
bool IsTestMode() const { return is_test_mode_; }
bool IsFmsAttached() const { return is_fms_attached_; }
bool IsAutonomous() const { return is_autonomous_; }
+ bool IsTeleop() const { return is_teleop_; }
+ bool IsDSAttached() const { return is_ds_attached_; }
bool IsSysActive() const;
bool IsBrownedOut() const;
+ std::string_view GetGameSpecificMessage() const;
+
+ std::string_view GetEventName() const;
+ MatchType GetMatchType() const;
+ int GetMatchNumber() const;
+ int GetReplayNumber() const;
Alliance GetAlliance() const;
int GetLocation() const;
double GetMatchTime() const;
@@ -88,6 +97,11 @@
bool is_test_mode_ = false;
bool is_autonomous_ = false;
bool is_fms_attached_ = false;
+ bool is_teleop_ = false;
+ bool is_ds_attached_ = false;
+
+ // statically allocated match info so we can return string_views into it.
+ HAL_MatchInfo info_;
};
} // namespace frc
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index fab3d05..9137a99 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -30,9 +30,6 @@
ds->RunIteration([&]() {
auto builder = joystick_state_sender_.MakeBuilder();
- HAL_MatchInfo match_info;
- auto status = HAL_GetMatchInfo(&match_info);
-
std::array<flatbuffers::Offset<Joystick>,
frc971::input::driver_station::JoystickFeature::kJoysticks>
joysticks;
@@ -67,32 +64,26 @@
joysticks_offset = builder.fbb()->CreateVector(joysticks.begin(),
joysticks.size());
- flatbuffers::Offset<flatbuffers::String> game_data_offset;
- if (status == 0) {
- static_assert(sizeof(match_info.gameSpecificMessage) == 64,
- "Check that the match info game specific message size "
- "hasn't changed and is still sane.");
- CHECK_LE(match_info.gameSpecificMessageSize,
- sizeof(match_info.gameSpecificMessage));
- game_data_offset = builder.fbb()->CreateString(
- reinterpret_cast<const char *>(match_info.gameSpecificMessage),
- match_info.gameSpecificMessageSize);
- }
+ flatbuffers::Offset<flatbuffers::String> game_data_offset =
+ builder.fbb()->CreateString(ds->GetGameSpecificMessage());
+
+ flatbuffers::Offset<flatbuffers::String> event_name_offset =
+ builder.fbb()->CreateString(ds->GetEventName());
aos::JoystickState::Builder joystick_state_builder =
builder.MakeBuilder<aos::JoystickState>();
joystick_state_builder.add_joysticks(joysticks_offset);
- if (status == 0) {
+ if (ds->GetGameSpecificMessage().size() >= 2u) {
joystick_state_builder.add_switch_left(
- match_info.gameSpecificMessage[0] == 'L' ||
- match_info.gameSpecificMessage[0] == 'l');
+ ds->GetGameSpecificMessage()[0] == 'L' ||
+ ds->GetGameSpecificMessage()[0] == 'l');
joystick_state_builder.add_scale_left(
- match_info.gameSpecificMessage[1] == 'L' ||
- match_info.gameSpecificMessage[1] == 'l');
- joystick_state_builder.add_game_data(game_data_offset);
+ ds->GetGameSpecificMessage()[1] == 'L' ||
+ ds->GetGameSpecificMessage()[1] == 'l');
}
+ joystick_state_builder.add_game_data(game_data_offset);
joystick_state_builder.add_test_mode(ds->IsTestMode());
joystick_state_builder.add_fms_attached(ds->IsFmsAttached());
@@ -109,7 +100,28 @@
joystick_state_builder.add_alliance(aos::Alliance::kInvalid);
break;
}
+ joystick_state_builder.add_location(ds->GetLocation());
+
joystick_state_builder.add_team_id(team_id_);
+ joystick_state_builder.add_match_number(ds->GetMatchNumber());
+ joystick_state_builder.add_replay_number(ds->GetReplayNumber());
+
+ switch (ds->GetMatchType()) {
+ case frc::DriverStation::kNone:
+ joystick_state_builder.add_match_type(aos::MatchType::kNone);
+ break;
+ case frc::DriverStation::kPractice:
+ joystick_state_builder.add_match_type(aos::MatchType::kPractice);
+ break;
+ case frc::DriverStation::kQualification:
+ joystick_state_builder.add_match_type(
+ aos::MatchType::kQualification);
+ break;
+ case frc::DriverStation::kElimination:
+ joystick_state_builder.add_match_type(aos::MatchType::kElimination);
+ break;
+ }
+ joystick_state_builder.add_event_name(event_name_offset);
if (builder.Send(joystick_state_builder.Finish()) !=
aos::RawSender::Error::kOk) {
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 1ee34f7..99890e4 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -242,7 +242,10 @@
superstructure_builder.add_roller_speed_compensation(1.5);
superstructure_builder.add_roller_speed_front(roller_front_voltage_);
superstructure_builder.add_roller_speed_back(roller_back_voltage_);
- superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
+ 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_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
superstructure_builder.add_preloaded(preloaded_);
@@ -257,28 +260,32 @@
void AutonomousActor::ExtendFrontIntake() {
set_intake_front_goal(kExtendIntakeGoal);
set_roller_front_voltage(kRollerVoltage);
- set_transfer_roller_voltage(kRollerVoltage);
+ set_transfer_roller_front_voltage(kRollerVoltage);
+ set_transfer_roller_back_voltage(-kRollerVoltage);
SendSuperstructureGoal();
}
void AutonomousActor::RetractFrontIntake() {
set_intake_front_goal(kRetractIntakeGoal);
set_roller_front_voltage(kRollerVoltage);
- set_transfer_roller_voltage(0.0);
+ set_transfer_roller_front_voltage(0.0);
+ set_transfer_roller_back_voltage(0.0);
SendSuperstructureGoal();
}
void AutonomousActor::ExtendBackIntake() {
set_intake_back_goal(kExtendIntakeGoal);
set_roller_back_voltage(kRollerVoltage);
- set_transfer_roller_voltage(-kRollerVoltage);
+ set_transfer_roller_back_voltage(kRollerVoltage);
+ set_transfer_roller_front_voltage(-kRollerVoltage);
SendSuperstructureGoal();
}
void AutonomousActor::RetractBackIntake() {
set_intake_back_goal(kRetractIntakeGoal);
set_roller_back_voltage(kRollerVoltage);
- set_transfer_roller_voltage(0.0);
+ set_transfer_roller_front_voltage(0.0);
+ set_transfer_roller_back_voltage(0.0);
SendSuperstructureGoal();
}
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index deeaa20..33922d7 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -40,8 +40,11 @@
void set_roller_back_voltage(double roller_back_voltage) {
roller_back_voltage_ = roller_back_voltage;
}
- void set_transfer_roller_voltage(double voltage) {
- transfer_roller_voltage_ = 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_fire_at_will(bool fire) { fire_ = fire; }
@@ -67,7 +70,8 @@
double intake_back_goal_ = 0.0;
double roller_front_voltage_ = 0.0;
double roller_back_voltage_ = 0.0;
- double transfer_roller_voltage_ = 0.0;
+ double transfer_roller_front_voltage_ = 0.0;
+ double transfer_roller_back_voltage_ = 0.0;
bool fire_ = false;
bool preloaded_ = false;
diff --git a/y2022/constants.h b/y2022/constants.h
index 494ca3d..c824076 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -48,9 +48,6 @@
return 22 * 0.25 * 0.0254;
}
static constexpr double kClimberPotRatio() { return 1.0; }
- // TODO(milind): figure this out
- // Climber position when it's comfortably above the mid rung.
- static constexpr double kClimberMidRungHeight() { return 1.0; }
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
@@ -97,33 +94,15 @@
};
}
- // When the intake is atleast this much out, always spin the rollers
- static constexpr double kIntakeSlightlyOutPosition() {
- return kIntakeRange().middle();
- }
- static constexpr double kIntakeOutPosition() { return 1.24; }
-
// Intake rollers
static constexpr double kIntakeRollerSupplyCurrentLimit() { return 40.0; }
static constexpr double kIntakeRollerStatorCurrentLimit() { return 60.0; }
// Transfer rollers
- // Positive voltage means front transfer rollers pull in and back spits out,
- // and vice versa
- static constexpr double kTransferRollerFrontVoltage() { return 12.0; }
- static constexpr double kTransferRollerBackVoltage() {
- return -kTransferRollerFrontVoltage();
- }
+ static constexpr double kTransferRollerVoltage() { return 12.0; }
// Voltage to wiggle the transfer rollers and keep a ball in.
- static constexpr double kTransferRollerFrontWiggleVoltage() { return 3.0; }
- static constexpr double kTransferRollerBackWiggleVoltage() {
- return -kTransferRollerFrontWiggleVoltage();
- }
- // Minimum roller speed when the intake is slightly out
- static constexpr double kMinIntakeSlightlyOutRollerSpeed() { return 6.0; }
- // Roller speeds when intake is out
- static constexpr double kIntakeOutRollerSpeed() { return 7.0; }
+ static constexpr double kTransferRollerWiggleVoltage() { return 3.0; }
// Turret
PotAndAbsEncoderConstants turret;
@@ -207,9 +186,6 @@
static constexpr double kCatapultEncoderCountsPerRevolution() {
return 4096.0;
}
- static constexpr double kDefaultCatapultShotPosition() { return 3.0; }
- static constexpr double kDefaultCatapultShotVelocity() { return 3.0; }
- static constexpr double kCatapultReturnPosition() { return -0.85; }
static constexpr double kMaxCatapultEncoderPulsesPerSecond() {
return control_loops::superstructure::catapult::kFreeSpeed / (2.0 * M_PI) *
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index cd54fea..7c53be3 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -87,19 +87,19 @@
void Simulate() {
FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
-
- // Don't simulate if already collided
- if (avoidance_.IsCollided(status_)) {
- return;
- }
+ bool was_collided = avoidance_.IsCollided(status_);
bool moving = true;
while (moving) {
// Compute the safe goal
avoidance_.UpdateGoal(status_, unsafe_goal_.message().turret());
- // The system should never be collided
- ASSERT_FALSE(avoidance_.IsCollided(status_));
+ if (!was_collided) {
+ // The system should never be collided if it didn't start off collided
+ EXPECT_FALSE(avoidance_.IsCollided(status_));
+ } else {
+ was_collided = avoidance_.IsCollided(status_);
+ }
safe_goal.mutable_message()->mutable_intake_front()->mutate_unsafe_goal(
::aos::Clip(intake_front_goal(), avoidance_.min_intake_front_goal(),
@@ -129,6 +129,8 @@
}
}
+ EXPECT_FALSE(avoidance_.IsCollided(status_));
+
CheckGoals();
}
@@ -336,8 +338,13 @@
{TurretState::kSafeFront, TurretState::kSafeBack,
TurretState::kSafeFrontWrapped, TurretState::kSafeBackWrapped,
TurretState::kUnsafeFront, TurretState::kUnsafeBack,
- TurretState::kUnsafeFrontWrapped,
- TurretState::kUnsafeBackWrapped}) {
+ TurretState::kUnsafeFrontWrapped, TurretState::kUnsafeBackWrapped,
+ TurretState::kNegativeSafeFront, TurretState::kNegativeSafeBack,
+ TurretState::kNegativeSafeFrontWrapped,
+ TurretState::kNegativeSafeBackWrapped,
+ TurretState::kNegativeUnsafeFront, TurretState::kNegativeUnsafeBack,
+ TurretState::kNegativeUnsafeFrontWrapped,
+ TurretState::kNegativeUnsafeBackWrapped}) {
// Intake front goal
for (IntakeState intake_front_goal :
{IntakeState::kSafe, IntakeState::kUnsafe}) {
@@ -350,7 +357,15 @@
TurretState::kSafeFrontWrapped, TurretState::kSafeBackWrapped,
TurretState::kUnsafeFront, TurretState::kUnsafeBack,
TurretState::kUnsafeFrontWrapped,
- TurretState::kUnsafeBackWrapped}) {
+ TurretState::kUnsafeBackWrapped,
+ TurretState::kNegativeSafeFront,
+ TurretState::kNegativeSafeBack,
+ TurretState::kNegativeSafeFrontWrapped,
+ TurretState::kNegativeSafeBackWrapped,
+ TurretState::kNegativeUnsafeFront,
+ TurretState::kNegativeUnsafeBack,
+ TurretState::kNegativeUnsafeFrontWrapped,
+ TurretState::kNegativeUnsafeBackWrapped}) {
// Catapult state
for (CatapultState catapult_state :
{CatapultState::kIdle, CatapultState::kShooting}) {
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 92e9528..e45917a 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();
@@ -128,9 +130,9 @@
}
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()) {
@@ -151,23 +153,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;
+ }
}
}
@@ -228,10 +236,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
@@ -403,7 +418,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 2868e98..8c7c862 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 e2e5c91..66a615e 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;
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 0065db7..ce5c755 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -157,7 +157,8 @@
// Default to the intakes in
double intake_front_pos = 1.47;
double intake_back_pos = 1.47;
- double transfer_roller_speed = 0.0;
+ double transfer_roller_front_speed = 0.0;
+ double transfer_roller_back_speed = 0.0;
double roller_front_speed = 0.0;
double roller_back_speed = 0.0;
@@ -198,11 +199,13 @@
if (data.IsPressed(kIntakeFrontOut)) {
intake_front_pos = 0.0;
roller_front_speed = 12.0;
- transfer_roller_speed = 12.0;
+ transfer_roller_front_speed = 12.0;
+ transfer_roller_back_speed = -transfer_roller_front_speed;
} else if (data.IsPressed(kIntakeBackOut)) {
roller_back_speed = 12.0;
intake_back_pos = 0.0;
- transfer_roller_speed = -12.0;
+ transfer_roller_back_speed = 12.0;
+ transfer_roller_front_speed = -transfer_roller_back_speed;
}
if (data.IsPressed(kFire)) {
@@ -253,7 +256,10 @@
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(transfer_roller_speed);
+ superstructure_goal_builder.add_transfer_roller_speed_front(
+ transfer_roller_front_speed);
+ superstructure_goal_builder.add_transfer_roller_speed_back(
+ transfer_roller_front_speed);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
diff --git a/y2022/setpoint_setter.cc b/y2022/setpoint_setter.cc
index 9a77ec8..8ec1015 100644
--- a/y2022/setpoint_setter.cc
+++ b/y2022/setpoint_setter.cc
@@ -2,15 +2,10 @@
#include "aos/init.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
-#include "y2022/constants.h"
#include "y2022/setpoint_generated.h"
-DEFINE_double(catapult_position,
- y2022::constants::Values::kDefaultCatapultShotPosition(),
- "Catapult shot position");
-DEFINE_double(catapult_velocity,
- y2022::constants::Values::kDefaultCatapultShotVelocity(),
- "Catapult shot velocity");
+DEFINE_double(catapult_position, 0.3, "Catapult shot position");
+DEFINE_double(catapult_velocity, 15.0, "Catapult shot velocity");
DEFINE_double(turret, 0.0, "Turret setpoint");
using y2022::input::joysticks::Setpoint;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index a694211..2294cda 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -215,8 +215,7 @@
frc971::PotAndAbsolutePositionT intake_back;
CopyPosition(intake_encoder_back_, &intake_back,
Values::kIntakeEncoderCountsPerRevolution(),
- Values::kIntakeEncoderRatio(),
- intake_pot_translate, true,
+ Values::kIntakeEncoderRatio(), intake_pot_translate, true,
values_->intake_back.potentiometer_offset);
frc971::PotAndAbsolutePositionT turret;
CopyPosition(turret_encoder_, &turret,
@@ -484,8 +483,12 @@
return flipper_arms_falcon_;
}
- void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
- transfer_roller_victor_ = ::std::move(t);
+ 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);
}
private:
@@ -500,7 +503,8 @@
ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
intake_falcon_front_->SetDisabled();
intake_falcon_back_->SetDisabled();
- transfer_roller_victor_->SetDisabled();
+ transfer_roller_victor_front_->SetDisabled();
+ transfer_roller_victor_back_->SetDisabled();
catapult_falcon_1_->SetDisabled();
turret_falcon_->SetDisabled();
}
@@ -512,7 +516,10 @@
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(), transfer_roller_victor_.get());
+ WritePwm(output.transfer_roller_voltage_front(),
+ transfer_roller_victor_front_.get());
+ WritePwm(output.transfer_roller_voltage_back(),
+ transfer_roller_victor_back_.get());
WriteCan(-output.flipper_arms_voltage(), flipper_arms_falcon_.get());
@@ -544,7 +551,8 @@
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
climber_falcon_;
- ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
+ ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_front_,
+ transfer_roller_victor_back_;
};
class CANSensorReader {
@@ -614,7 +622,7 @@
// Thread 2.
::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
- ;AddLoop(&pdp_fetcher_event_loop);
+ AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
@@ -639,8 +647,7 @@
sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(5));
// TODO(milind): correct intake beambreak ports once set
- sensor_reader.set_intake_beambreak_front(
- make_unique<frc::DigitalInput>(1));
+ sensor_reader.set_intake_beambreak_front(make_unique<frc::DigitalInput>(1));
sensor_reader.set_intake_beambreak_back(make_unique<frc::DigitalInput>(6));
sensor_reader.set_turret_beambreak(make_unique<frc::DigitalInput>(7));
@@ -679,8 +686,9 @@
superstructure_writer.set_roller_falcon_back(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
- // TODO(milind): correct port
- superstructure_writer.set_transfer_roller_victor(
+ superstructure_writer.set_transfer_roller_victor_front(
+ make_unique<::frc::VictorSP>(6));
+ superstructure_writer.set_transfer_roller_victor_back(
make_unique<::frc::VictorSP>(5));
superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(2));