Merge "Give superstructure_lib_test more time to run"
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 784b92f..f3015c6 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -78,7 +78,7 @@
.frc971.HallEffectAndPositionEstimatorState estimator_state;
};
-struct AbsoluteProfiledJointStatus {
+struct PotAndAbsoluteEncoderProfiledJointStatus {
// Is the subsystem zeroed?
bool zeroed;
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 52bd8d1..9748a41 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -17,16 +17,16 @@
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-LENGTH_OF_FIELD = 323.65
+WIDTH_OF_FIELD_IN_METERS = 8.258302
PIXELS_ON_SCREEN = 300
-def pxToIn(p):
- return p * LENGTH_OF_FIELD / PIXELS_ON_SCREEN
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-def inToPx(i):
- return (i * PIXELS_ON_SCREEN / LENGTH_OF_FIELD)
+def mToPx(i):
+ return (i * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
def px(cr):
@@ -155,7 +155,7 @@
#John also wrote this
def add_point(self, x, y):
if (len(self.selected_points) < 6):
- self.selected_points.append([x, y])
+ self.selected_points.append([pxToM(x), pxToM(y)])
if (len(self.selected_points) == 6):
self.mode = Mode.kEditing
self.splines.append(np.array(self.selected_points))
@@ -188,9 +188,10 @@
cur_p = [self.x, self.y]
for index_splines, points in enumerate(self.spline):
- for index_points, i in enumerate(points.curve):
+ for index_points, point in enumerate(points.curve):
# pythagorean
- distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
+ distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
+ (cur_p[1] - mToPx(point[1])**2))
if distance < nearest:
nearest = distance
print("DISTANCE: ", distance, " | INDEX: ", index_points)
@@ -222,7 +223,7 @@
if index > 0 and index <= self.index_of_edit:
distance += np.sqrt((points[index - 1][0] - point[0])**2 +
(points[index - 1][1] - point[1])**2)
- return pxToIn(distance)
+ return distance
# Handle the expose-event by updating the Window and drawing
def handle_draw(self, cr):
@@ -255,23 +256,6 @@
cr.rectangle(-450, -150, 300, 300)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
- cr.rectangle((inToPx(140 - 161.825) - 300), inToPx(76.575), inToPx(56),
- inToPx(-153.15))
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
- cr.stroke()
- cr.rectangle((inToPx(161.825 - 24) - 300), inToPx(90), inToPx(24),
- inToPx(-180))
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
- cr.stroke()
-
- set_color(cr, Color(0.2, 0.2, 0.2))
- cr.rectangle(
- inToPx(140 - 161.825) - 300, inToPx(76.575), inToPx(56),
- inToPx(-153.15))
- cr.fill()
- cr.rectangle(
- inToPx(161.825 - 24) - 300, inToPx(90), inToPx(24), inToPx(-180))
- cr.fill()
y = 0
@@ -307,8 +291,8 @@
print(str(item))
for i, point in enumerate(self.selected_points):
print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
+ cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
elif self.mode == Mode.kEditing:
@@ -316,17 +300,14 @@
cr.move_to(-300, 170)
display_text(cr, "EDITING", 1, 1, 1, 1)
if len(self.splines) > 0:
- # print("Splines: " + str(len(self.splines)))
- # print("ITEMS:")
holder_spline = []
for i, points in enumerate(self.splines):
array = np.zeros(shape=(6, 2), dtype=float)
for j, point in enumerate(points):
- array[j, 0] = point[0]
- array[j, 1] = point[1]
+ array[j, 0] = mToPx(point[0])
+ array[j, 1] = mToPx(point[1])
spline = Spline(np.ascontiguousarray(np.transpose(array)))
for k in np.linspace(0.01, 1, 100):
-
cr.move_to(
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1])
@@ -336,7 +317,6 @@
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1]
]
-
holder_spline.append(holding)
self.curves.append(holder_spline)
@@ -346,13 +326,13 @@
for i, point in enumerate(points):
# print("I: " + str(i))
if spline == self.spline_edit and i == self.index_of_edit:
- draw_px_x(cr, point[0], point[1], 15,
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
self.colors[spline])
elif (spline == 0 and not i == 5) or (not i == 0
and not i == 5):
- draw_px_x(cr, point[0], point[1], 10,
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
self.colors[spline])
- cr.move_to(point[0], point[1] - 15)
+ cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
elif self.mode == Mode.kExporting:
@@ -466,7 +446,7 @@
if self.index_of_edit > -1 and self.held_x != self.x:
print("INDEX OF EDIT: " + str(self.index_of_edit))
self.splines[self.spline_edit][self.index_of_edit] = [
- self.x, self.y
+ pxToM(self.x), pxToM(self.y)
]
if not self.spline_edit == len(self.splines) - 1:
@@ -494,11 +474,11 @@
# Get clicked point
# Find nearest
# Move nearest to clicked
- cur_p = [self.x, self.y]
+ cur_p = [pxToM(self.x), pxToM(self.y)]
print("CUR_P: " + str(self.x) + " " + str(self.y))
# Get the distance between each for x and y
# Save the index of the point closest
- nearest = 50
+ nearest = 1 # Max distance away a the selected point can be in meters
index_of_closest = 0
for index_splines, points in enumerate(self.splines):
for index_points, val in enumerate(points):
@@ -520,7 +500,7 @@
def do_button_press(self, event):
print("button press activated")
- #Be consistent with the scaling in the drawing_area
+ # Be consistent with the scaling in the drawing_area
self.x = event.x * 2
self.y = event.y * 2
self.button_press_action()
@@ -622,4 +602,4 @@
window = GridWindow()
-basic_window.RunApp()
\ No newline at end of file
+basic_window.RunApp()
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 9ad7439..2709315 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -65,7 +65,8 @@
void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position,
double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *status);
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -119,7 +120,7 @@
void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 8805842..df26c56 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -62,7 +62,7 @@
struct TestIntakeSystemData {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
- ::frc971::control_loops::AbsoluteProfiledJointStatus status;
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus status;
::frc971::PotAndAbsolutePosition position;
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index 89145a4..76c44a4 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -32,7 +32,7 @@
void Intake::Iterate(
const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index 40f49fc..db1442a 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -34,7 +34,8 @@
void Iterate(const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *status);
void Reset();
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 60c5e16..5c0771c 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -200,7 +200,7 @@
bool estopped;
// Each subsystems status.
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
.frc971.control_loops.IndexProfiledJointStatus hood;
ShooterStatus shooter;
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 5d07e17..453c680 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -39,3 +39,31 @@
"//aos/events:shm-event-loop",
],
)
+
+cc_library(
+ name = "collision_avoidance",
+ srcs = [
+ "collision_avoidance.cc",
+ ],
+ hdrs = [
+ "collision_avoidance.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop_queues",
+ "//frc971:constants",
+ ],
+)
+
+cc_test(
+ name = "collision_avoidance_tests",
+ srcs = [
+ "collision_avoidance_tests.cc",
+ ],
+ deps = [
+ ":collision_avoidance",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..7a26b90
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,162 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double CollisionAvoidance::kElevatorClearHeight;
+constexpr double CollisionAvoidance::kElevatorClearWristDownHeight;
+constexpr double CollisionAvoidance::kElevatorClearIntakeHeight;
+constexpr double CollisionAvoidance::kWristMaxAngle;
+constexpr double CollisionAvoidance::kWristMinAngle;
+constexpr double CollisionAvoidance::kIntakeOutAngle;
+constexpr double CollisionAvoidance::kIntakeInAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMinAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMaxAngle;
+constexpr double CollisionAvoidance::kEps;
+constexpr double CollisionAvoidance::kEpsIntake;
+constexpr double CollisionAvoidance::kEpsWrist;
+
+CollisionAvoidance::CollisionAvoidance() {
+ clear_min_wrist_goal();
+ clear_max_wrist_goal();
+ clear_min_elevator_goal();
+ clear_min_intake_goal();
+ clear_max_intake_goal();
+}
+
+bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Elevator is down, so the wrist can't be close to vertical.
+ if (elevator_position < kElevatorClearHeight) {
+ if (wrist_position < kWristElevatorCollisionMaxAngle &&
+ wrist_position > kWristElevatorCollisionMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so wrist can't go below horizontal in either direction.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ if (wrist_position > kWristMaxAngle) {
+ return true;
+ }
+ if (wrist_position < kWristMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so the intake has to be at either extreme.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
+ return true;
+ }
+ }
+
+ // Nothing is hitting, we must be good.
+ return false;
+}
+
+void CollisionAvoidance::UpdateGoal(
+ const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Start with our constraints being wide open.
+ clear_max_wrist_goal();
+ clear_min_wrist_goal();
+ clear_max_intake_goal();
+ clear_min_intake_goal();
+
+ // If the elevator is low enough, we also can't transition the wrist.
+ if (elevator_position < kElevatorClearHeight) {
+ // Figure out which side the wrist is on and stay there.
+ if (wrist_position < 0.0) {
+ update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
+ } else {
+ update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
+ }
+ }
+
+ // If the elevator is too low, the wrist needs to be above the clearance
+ // angles to avoid crashing the frame.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ update_min_wrist_goal(kWristMinAngle + kEpsWrist);
+ update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+ }
+
+ constexpr double kIntakeMiddleAngle =
+ (kIntakeOutAngle + kIntakeInAngle) / 2.0;
+
+ // If the elevator is too low, the intake can't transition from in to out or
+ // back.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ // Figure out if the intake is in our out and keep it there.
+ if (intake_position < kIntakeMiddleAngle) {
+ update_max_intake_goal(kIntakeInAngle - kEpsIntake);
+ } else {
+ update_min_intake_goal(kIntakeOutAngle + kEpsIntake);
+ }
+ }
+
+ // Start with an unconstrained elevator.
+ clear_min_elevator_goal();
+
+ // If the intake is within the collision range, don't let the elevator down.
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+
+ // If the wrist is within the elevator collision range, don't let the elevator
+ // go down.
+ if (wrist_position > kWristElevatorCollisionMinAngle &&
+ wrist_position < kWristElevatorCollisionMaxAngle) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // If the wrist is far enough down that we are going to hit the frame, don't
+ // let the elevator go too far down.
+ if (wrist_position > kWristMaxAngle || wrist_position < kWristMinAngle) {
+ update_min_elevator_goal(kElevatorClearWristDownHeight + kEps);
+ }
+
+ if (unsafe_goal) {
+ const double wrist_goal = unsafe_goal->wrist.angle;
+ const double intake_goal = unsafe_goal->intake.joint_angle;
+
+ // Compute if we need to move the intake.
+ const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
+ (intake_goal < kIntakeMiddleAngle);
+
+ // Compute if we need to move the wrist across 0.
+ const bool wrist_needs_to_move =
+ (wrist_position < 0.0) ^ (wrist_goal < 0.0);
+
+ // If we need to move the intake, we've got to shove the elevator up. The
+ // intake is already constrained so it can't hit anything until it's clear.
+ if (intake_needs_to_move && wrist_position > 0) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+ // If we need to move the wrist, we've got to shove the elevator up too. The
+ // wrist is already constrained so it can't hit anything until it's clear.
+ // If both the intake and wrist need to move, figure out which one will
+ // require the higher motion and move that.
+ if (wrist_needs_to_move) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // TODO(austin): We won't shove the elevator up if the wrist is asked to go
+ // down below horizontal. I think that's fine.
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..1ad9582
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,111 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+
+#include <cmath>
+#include "aos/controls/control_loops.q.h"
+#include "frc971/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+// CollisionAvoidance computes the min and max allowable ranges for various
+// subsystems to avoid collisions. It also shoves the elevator up to let the
+// intake go in and out, and to let the wrist switch sides.
+class CollisionAvoidance {
+ public:
+ CollisionAvoidance();
+
+ // Reports if the superstructure is collided.
+ bool IsCollided(const SuperstructureQueue::Status *status);
+
+ // Checks and alters goals to make sure they're safe.
+ // TODO(austin): Either we will have a unit delay because this has to happen
+ // after the controls, or we need to be more clever about restructuring.
+ void UpdateGoal(const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal);
+
+ // Returns the goals to give to the respective control loops in
+ // superstructure.
+ double min_wrist_goal() const { return min_wrist_goal_; }
+ double max_wrist_goal() const { return max_wrist_goal_; }
+ double min_elevator_goal() const { return min_elevator_goal_; }
+ double min_intake_goal() const { return min_intake_goal_; }
+ double max_intake_goal() const { return max_intake_goal_; }
+
+ void update_max_wrist_goal(double max_wrist_goal) {
+ max_wrist_goal_ = ::std::min(max_wrist_goal, max_wrist_goal_);
+ }
+ void update_min_wrist_goal(double min_wrist_goal) {
+ min_wrist_goal_ = ::std::max(min_wrist_goal, min_wrist_goal_);
+ }
+ void update_max_intake_goal(double max_intake_goal) {
+ max_intake_goal_ = ::std::min(max_intake_goal, max_intake_goal_);
+ }
+ void update_min_intake_goal(double min_intake_goal) {
+ min_intake_goal_ = ::std::max(min_intake_goal, min_intake_goal_);
+ }
+ void update_min_elevator_goal(double min_elevator_goal) {
+ min_elevator_goal_ = ::std::max(min_elevator_goal, min_elevator_goal_);
+ }
+
+ // TODO(sabina): set all the numbers to correctly match the robot.
+
+ // Height above which we can move the wrist freely.
+ static constexpr double kElevatorClearHeight = 0.5;
+
+ // Height above which we can move the wrist down.
+ static constexpr double kElevatorClearWristDownHeight = 0.3;
+ // Height the carriage needs to be above to move the intake.
+ static constexpr double kElevatorClearIntakeHeight = 0.4;
+
+ // Angle constraints for the wrist when below kElevatorClearDownHeight
+ static constexpr double kWristMaxAngle = M_PI / 2.0;
+ static constexpr double kWristMinAngle = -M_PI / 2.0;
+
+ // Angles outside of which the intake is fully clear of the wrist.
+ static constexpr double kIntakeOutAngle = M_PI / 6.0;
+ static constexpr double kIntakeInAngle = -M_PI / 3.0;
+
+ // Angles within which we will crash the wrist into the elevator if the
+ // elevator is below kElevatorClearHeight.
+ static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
+ static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
+
+ // Tolerance for the elevator.
+ static constexpr double kEps = 0.02;
+ // Tolerance for the intake.
+ static constexpr double kEpsIntake = 0.05;
+ // Tolerance for the wrist.
+ static constexpr double kEpsWrist = 0.05;
+
+ private:
+ void clear_min_wrist_goal() {
+ min_wrist_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_wrist_goal() {
+ max_wrist_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_elevator_goal() {
+ min_elevator_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_intake_goal() {
+ min_intake_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_goal() {
+ max_intake_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+
+ double min_wrist_goal_;
+ double max_wrist_goal_;
+ double min_elevator_goal_;
+ double min_intake_goal_;
+ double max_intake_goal_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
new file mode 100644
index 0000000..834a251
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -0,0 +1,281 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include "aos/commonmath.h"
+#include "gtest/gtest.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+/*
+Test List:
+ FullClockwiseRotationFromBottomBackIntakeIn
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle
+ QuarterClockwiseRotationFromMiddleFrontIntakeMoving
+
+ FullCounterClockwiseRotationFromBottomFrontIntakeIn
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeOut
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
+*/
+
+class CollisionAvoidanceTests : public ::testing::Test {
+ public:
+ void Iterate() {
+ SuperstructureQueue::Goal safe_goal;
+ while (true) {
+ avoidance.UpdateGoal(&status, &unsafe_goal);
+
+ EXPECT_FALSE(avoidance.IsCollided(&status));
+ safe_goal.wrist.angle =
+ ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+ avoidance.max_wrist_goal());
+
+ safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+ avoidance.min_elevator_goal());
+
+ safe_goal.intake.joint_angle =
+ ::aos::Clip(unsafe_goal.intake.joint_angle,
+ avoidance.min_intake_goal(), avoidance.max_intake_goal());
+
+ LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
+ LimitedMove(&status.elevator.position, safe_goal.elevator.height);
+ LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+ if (IsMoving()) {
+ break;
+ }
+ past_status = status;
+ }
+ }
+
+ bool IsMoving() {
+ if ((past_status.wrist.position == status.wrist.position) &&
+ (past_status.elevator.position == status.elevator.position) &&
+ (past_status.intake.position == status.intake.position)) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // provide goals and status messages
+ SuperstructureQueue::Goal unsafe_goal;
+ SuperstructureQueue::Status status;
+ SuperstructureQueue::Status past_status;
+
+ protected:
+ // setup for all tests
+ CollisionAvoidance avoidance;
+
+ void CheckGoals() {
+ // check to see if we reached the goals
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ }
+
+ private:
+ void LimitedMove(float *position, double goal) {
+ if (*position + kIterationMove < goal) {
+ *position += kIterationMove;
+ } else if (*position - kIterationMove > goal) {
+ *position -= kIterationMove;
+ } else {
+ *position = goal;
+ }
+ }
+
+ static constexpr double kIterationMove = 0.001;
+};
+
+// It is trying to rotate from far back to front low.
+TEST_F(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle;
+
+ Iterate();
+
+ CheckGoals();
+}
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ status.wrist.position = avoidance.kWristMaxAngle / 2.0;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from front low to far back.
+TEST_F(CollisionAvoidanceTests,
+ FullCounterClockwiseRotationFromBottomBackIntakeIn) {
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// Unreasonable Elevator Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = 4.0;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.45;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Unreasonable Wrist Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableWristGoal) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.45;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index 2cd7238..fef9ced 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -67,10 +67,10 @@
bool has_piece;
// Status of each subsystem.
- .frc971.control_loops.AbsoluteProfiledJointStatus elevator;
- .frc971.control_loops.AbsoluteProfiledJointStatus wrist;
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
- .frc971.control_loops.AbsoluteProfiledJointStatus stilts;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
};
message Position {