Add roller velocity compensation
When we are moving faster forwards, we want the roller to be spinning
faster to compensate for the ground velocity. Otherwise we push balls
away. This also makes it so we don't throw them as hard when moving
slow.
Added tests for intake roller voltage
Change-Id: Ica015d124c7a192bcd001dbc02330f71d6616b04
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 0d740e9..4c02a18 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -24,6 +24,12 @@
event_loop->SetRuntimeRealtimePriority(30);
}
+double Superstructure::robot_speed() const {
+ return (drivetrain_status_fetcher_.get() != nullptr
+ ? drivetrain_status_fetcher_->robot_speed()
+ : 0.0);
+}
+
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
aos::Sender<Output>::Builder *output,
@@ -52,6 +58,8 @@
turret::Aimer::ShotMode::kShootOnTheFly);
}
+ const float velocity = robot_speed();
+
const flatbuffers::Offset<AimerStatus> aimer_status_offset =
aimer_.PopulateStatus(status->fbb());
@@ -131,7 +139,8 @@
turret_.operating_voltage());
// Friction is a pain and putting a really high burden on the integrator.
- const double hood_velocity_sign = hood_status->velocity() * kHoodFrictionGain;
+ const double hood_velocity_sign =
+ hood_status->velocity() * kHoodFrictionGain;
output_struct.hood_voltage +=
std::clamp(hood_velocity_sign, -kHoodFrictionVoltageLimit,
kHoodFrictionVoltageLimit);
@@ -182,7 +191,9 @@
output_struct.intake_roller_voltage = 3.0;
} else {
output_struct.feeder_voltage = 0.0;
- output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
+ output_struct.intake_roller_voltage =
+ unsafe_goal->roller_voltage() +
+ std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0f);
}
} else {
output_struct.intake_roller_voltage = 0.0;
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 1a9d401..7b87bca 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -45,6 +45,7 @@
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
const shooter::Shooter &shooter() const { return shooter_; }
+ double robot_speed() const;
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index c7fe643..9200082 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -14,40 +14,43 @@
table Goal {
// Zero is at the horizontal, positive towards the front (meters on the lead screw).
// Only applies if hood_tracking = false.
- hood:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ hood:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
// Positive = counterclockwise from above; rotates Wheel of Fortune clockwise
// Zero is relative to start.
- control_panel:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ control_panel:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 1);
// 0 = Linkage on sprocket is pointing straight up
// Positive = forward
- intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
// Positive is rollers intaking to Washing Machine.
- roller_voltage:float;
+ roller_voltage:float (id: 3);
+
+ // Factor to multiply robot velocity by and add to roller voltage.
+ roller_speed_compensation:float (id: 11);
// 0 = facing the back of the robot. Positive rotates counterclockwise.
- turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 4);
// Only applies if shooter_tracking = false.
- shooter:ShooterGoal;
+ shooter:ShooterGoal (id: 5);
// Whether the robot should be shooting balls. Waits until hood, turret, and
// shooter are at goal (as determined by auto-tracking or override).
- shooting:bool;
+ shooting:bool (id: 6);
// Whether the hood should adjust its position automatically.
- hood_tracking:bool;
+ hood_tracking:bool (id: 7);
// Whether the turret should follow the target automatically.
- turret_tracking:bool;
+ turret_tracking:bool (id: 8);
// Whether the kicker and flywheel should choose a velocity automatically.
- shooter_tracking:bool;
+ shooter_tracking:bool (id: 9);
// Positive is deploying climber and to climb; cannot run in reverse
- climber_voltage:float;
+ climber_voltage:float (id: 10);
}
root_type Goal;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 8a0589c..ffa9539 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -528,6 +528,27 @@
!superstructure_status_fetcher_.get()->zeroed());
}
+ // Method for testing the intake_roller_voltage
+ void TestIntakeRollerVoltage(const float roller_voltage,
+ const float roller_speed_compensation,
+ const bool shooting,
+ const double expected_voltage) {
+ SetEnabled(true);
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_roller_voltage(roller_voltage);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ goal_builder.add_shooting(shooting);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+ RunFor(chrono::seconds(1));
+ superstructure_output_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(),
+ expected_voltage);
+ }
+
const aos::Node *const roborio_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -859,6 +880,30 @@
VerifyNearGoal();
}
+// Makes sure that a negative number is not added to the to the
+// roller_voltage
+TEST_F(SuperstructureTest, NegativeRollerSpeedCompensation) {
+ constexpr float kRollerVoltage = 12.0f;
+ TestIntakeRollerVoltage(kRollerVoltage, -10.0f, false, kRollerVoltage);
+}
+
+// Makes sure that intake_roller_voltage is correctly being calculated
+// based on the velocity when the roller_speed_compensation is positive
+TEST_F(SuperstructureTest, PositiveRollerSpeedCompensation) {
+ constexpr float kRollerVoltage = 12.0f;
+ constexpr float kRollerSpeedCompensation = 10.0f;
+ TestIntakeRollerVoltage(kRollerVoltage, kRollerSpeedCompensation, false,
+ kRollerVoltage + (superstructure_.robot_speed() *
+ kRollerSpeedCompensation));
+}
+
+// Makes sure that the intake_roller_voltage is 3.0
+// when the robot should be shooting balls (after the hood, turret, and
+// shooter at at the goal)
+TEST_F(SuperstructureTest, WhenShooting) {
+ TestIntakeRollerVoltage(0.0f, 0.0f, true, 3.0);
+}
+
class SuperstructureAllianceTest
: public SuperstructureTest,
public ::testing::WithParamInterface<aos::Alliance> {};
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 5825475..1cd3295 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -79,6 +79,7 @@
double intake_pos = -0.89;
double turret_pos = 0.0;
float roller_speed = 0.0f;
+ float roller_speed_compensation = 0.0f;
double accelerator_speed = 0.0;
double finisher_speed = 0.0;
double climber_speed = 0.0;
@@ -107,24 +108,24 @@
}
} else if (data.IsPressed(kShootSlow)) {
accelerator_speed = 180.0;
- finisher_speed = 375.0;
+ finisher_speed = 300.0;
}
if (data.IsPressed(kIntakeExtend)) {
intake_pos = 1.2;
- roller_speed = 9.0f;
+ roller_speed = 7.0f;
+ roller_speed_compensation = 2.0f;
}
if (superstructure_status_fetcher_.get() &&
superstructure_status_fetcher_->intake()->position() > -0.5) {
roller_speed = std::max(roller_speed, 6.0f);
- }
-
- if (data.IsPressed(kFeed)) {
+ roller_speed_compensation = 2.0f;
}
if (data.IsPressed(kIntakeIn)) {
roller_speed = 6.0f;
+ roller_speed_compensation = 2.0f;
} else if (data.IsPressed(kSpit)) {
roller_speed = -6.0f;
}
@@ -163,6 +164,8 @@
superstructure_goal_builder.add_intake(intake_offset);
superstructure_goal_builder.add_turret(turret_offset);
superstructure_goal_builder.add_roller_voltage(roller_speed);
+ superstructure_goal_builder.add_roller_speed_compensation(
+ roller_speed_compensation);
superstructure_goal_builder.add_shooter(shooter_offset);
superstructure_goal_builder.add_shooting(data.IsPressed(kFeed));
superstructure_goal_builder.add_climber_voltage(climber_speed);