Tune up the catapult state machine to work
This now holds the ball in the intake, and will transfer it when the
turret is in position.
Change-Id: I2e9aa11cae014b56679edaeb9f71a7c32664094c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
diff --git a/y2022/constants.h b/y2022/constants.h
index 5b8fe64..867f239 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -116,7 +116,7 @@
}
// Voltage to wiggle the transfer rollers and keep a ball in.
- static constexpr double kTransferRollerFrontWiggleVoltage() { return 5.0; }
+ static constexpr double kTransferRollerFrontWiggleVoltage() { return 3.0; }
static constexpr double kTransferRollerBackWiggleVoltage() {
return -kTransferRollerFrontWiggleVoltage();
}
@@ -138,8 +138,8 @@
};
}
- static constexpr double kTurretBackIntakePos() { return 0.0; }
- static constexpr double kTurretFrontIntakePos() { return M_PI; }
+ static constexpr double kTurretBackIntakePos() { return M_PI; }
+ static constexpr double kTurretFrontIntakePos() { return 0; }
static constexpr double kTurretPotRatio() { return 27.0 / 110.0; }
static constexpr double kTurretEncoderRatio() { return kTurretPotRatio(); }
@@ -161,7 +161,7 @@
static constexpr double kFlipperHoldVoltage() { return 2.5; }
// Voltage to feed a ball from the transfer rollers to the catpult with the
// flippers
- static constexpr double kFlipperFeedVoltage() { return -8.0; }
+ static constexpr double kFlipperFeedVoltage() { return -12.0; }
// Ball is fed into catapult for atleast this time no matter what
static constexpr std::chrono::milliseconds kExtraLoadingTime() {
@@ -218,9 +218,9 @@
}
static constexpr ::frc971::constants::Range kCatapultRange() {
return ::frc971::constants::Range{
- .lower_hard = -1.2,
+ .lower_hard = -1.0,
.upper_hard = 2.0,
- .lower = -1.00,
+ .lower = -0.90,
.upper = 1.57,
};
}
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index affc25b..013db30 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -162,8 +162,6 @@
}
state_ = SuperstructureState::TRANSFERRING;
- transferring_timer_ = timestamp;
-
// Save the side the ball is on for later
break;
@@ -171,8 +169,9 @@
case SuperstructureState::TRANSFERRING: {
// If we've been transferring for too long, the ball probably got lost
if (timestamp >
- transferring_timer_ + constants::Values::kBallLostTime()) {
+ intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
intake_state_ = IntakeState::NO_BALL;
+ state_ = SuperstructureState::IDLE;
break;
}
@@ -219,26 +218,13 @@
// Keep feeding for kExtraLoadingTime
- can_position_fetcher_.Fetch();
- const bool flipper_arm_roller_is_stopped =
- can_position_fetcher_.get() != nullptr &&
- std::abs(
- can_position_fetcher_->flipper_arm_integrated_sensor_velocity()) <
- 0.01;
-
- const bool reading_is_recent =
- can_position_fetcher_.get() != nullptr &&
- (timestamp < can_position_fetcher_.context().monotonic_event_time +
- constants::Values::kFlipperVelocityValidTime());
-
// The ball should go past the turret beambreak to be loaded.
// If we got a CAN reading not too long ago, the flippers should have also
// stopped.
- // TODO(milind): maybe it's better to update loading_timer_ as long as the
- // turret beambreak is triggered.
- if (timestamp > loading_timer_ + constants::Values::kExtraLoadingTime() &&
- !position->turret_beambreak() &&
- (flipper_arm_roller_is_stopped || !reading_is_recent)) {
+ if (position->turret_beambreak()) {
+ loading_timer_ = timestamp;
+ } else if (timestamp >
+ loading_timer_ + constants::Values::kExtraLoadingTime()) {
state_ = SuperstructureState::LOADED;
reseating_in_catapult_ = false;
}
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 13a790a..03afaf9 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -30,8 +30,8 @@
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
- static constexpr double kTurretGoalThreshold = 0.01;
- static constexpr double kCatapultGoalThreshold = 0.01;
+ static constexpr double kTurretGoalThreshold = 0.05;
+ static constexpr double kCatapultGoalThreshold = 0.05;
// potentiometer will be more noisy
static constexpr double kFlipperGoalThreshold = 0.05;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 7425ac0..2290397 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -852,7 +852,7 @@
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
superstructure_plant_.set_turret_beambreak(false);
- RunFor(constants::Values::kExtraLoadingTime() + dt());
+ RunFor(constants::Values::kExtraLoadingTime() + dt() * 2);
// Now that the ball has gone past the turret beambreak,
// it should be loaded in the catapult and ready for firing.