Flip intake direction in collision avoidance
It makes much more sense that 0 is when it's fully down, and positive
is moving up.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I7419c7c942ca99ccdba4bf6e61bad174b67c0cc9
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index b39b48f..01b0463 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -58,7 +58,7 @@
if (turret_position_wrapped >= min_turret_collision_position &&
turret_position_wrapped <= max_turret_collision_position) {
// Reterns true if the intake is raised.
- if (intake_position <= kCollisionZoneIntake) {
+ if (intake_position > kCollisionZoneIntake) {
return true;
}
} else {
@@ -144,14 +144,14 @@
if (turret_pos_unsafe || turret_moving_past_intake) {
// If the turret is unsafe, limit the intake
if (intake_front) {
- update_min_intake_front_goal(kCollisionZoneIntake + kEpsIntake);
+ update_max_intake_front_goal(kCollisionZoneIntake - kEpsIntake);
} else {
- update_min_intake_back_goal(kCollisionZoneIntake + kEpsIntake);
+ update_max_intake_back_goal(kCollisionZoneIntake - kEpsIntake);
}
// If the intake is in the way, limit the turret until moved. Otherwise,
// let'errip!
- if (!turret_pos_unsafe && (intake_position <= kCollisionZoneIntake)) {
+ if (!turret_pos_unsafe && (intake_position > kCollisionZoneIntake)) {
if (turret_position < min_turret_collision_goal_unwrapped) {
update_max_turret_goal(min_turret_collision_goal_unwrapped -
kEpsTurret);
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index 7c25964..ec7ce6f 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -52,7 +52,7 @@
static constexpr double kMinCollisionZoneBackTurret = -kCollisionZoneTurret;
static constexpr double kMaxCollisionZoneBackTurret = kCollisionZoneTurret;
- // Minimum (highest in reality) of the intake, in order to avoid collisions
+ // Maximum position of the intake to avoid collisions
static constexpr double kCollisionZoneIntake = M_PI / 6.0;
// Tolerance for the turret.
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index 43695d1..adf672b 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -138,11 +138,11 @@
switch (intake_state) {
case IntakeState::kSafe:
- intake_angle = CollisionAvoidance::kCollisionZoneIntake +
+ intake_angle = CollisionAvoidance::kCollisionZoneIntake -
CollisionAvoidance::kEpsIntake;
break;
case IntakeState::kUnsafe:
- intake_angle = CollisionAvoidance::kCollisionZoneIntake -
+ intake_angle = CollisionAvoidance::kCollisionZoneIntake +
CollisionAvoidance::kEpsIntake;
break;
}
@@ -267,7 +267,7 @@
intake_front_goal(), turret_goal(),
CollisionAvoidance::kMinCollisionZoneFrontTurret,
CollisionAvoidance::kMaxCollisionZoneFrontTurret)) {
- EXPECT_GE(status_.intake_front_position,
+ EXPECT_LE(status_.intake_front_position,
CollisionAvoidance::kCollisionZoneIntake);
} else {
EXPECT_NEAR(intake_front_goal(), status_.intake_front_position,
@@ -278,7 +278,7 @@
intake_back_goal(), turret_goal(),
CollisionAvoidance::kMinCollisionZoneBackTurret,
CollisionAvoidance::kMaxCollisionZoneBackTurret)) {
- EXPECT_GE(status_.intake_back_position,
+ EXPECT_LE(status_.intake_back_position,
CollisionAvoidance::kCollisionZoneIntake);
} else {
EXPECT_NEAR(intake_back_goal(), status_.intake_back_position, 0.001);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index ebd0bd3..7425ac0 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -570,22 +570,22 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().upper,
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().upper,
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().upper,
+ *builder.fbb(), constants::Values::kTurretRange().lower,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kClimberRange().upper,
+ *builder.fbb(), constants::Values::kClimberRange().lower,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -620,11 +620,11 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().lower);
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().lower);
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
// Keep the turret away from the intakes because they start in the collision
// area
@@ -634,7 +634,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kClimberRange().lower);
+ *builder.fbb(), constants::Values::kClimberRange().upper);
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -657,22 +657,22 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().upper,
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().upper,
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().upper,
+ *builder.fbb(), constants::Values::kTurretRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kClimberRange().upper,
+ *builder.fbb(), constants::Values::kClimberRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();