FridgeKinematics:
- Added class to perform forward and inverse kinematics.
- Added basic test to hand verify results.
- Added claw fridge collision check by Mr. Schuh.
- Revered code review changes that broke the math.
- Added tests for collision from Mr. Schuh.
- Constants are working, thanks Brian.
- Added velocity calculation based on jacobian.
Change-Id: Iba752fbe6ebecca6dfd3102b23c8aebc3cc652a2
diff --git a/frc971/constants.h b/frc971/constants.h
index d66853b..fe47418 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -116,6 +116,29 @@
double max_allowed_left_right_arm_difference;
double max_allowed_left_right_elevator_difference;
+
+ struct ClawGeometry {
+ // Horizontal distance from the center of the grabber to the end.
+ double grabber_half_length;
+ // Vertical distance from the arm rotation center to the bottom of the
+ // grabber. Distance measured with arm vertical (theta = 0).
+ double grabber_delta_y;
+ // Vertical separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ double grabber_arm_vert_separation;
+ // Horizontal separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ double grabber_arm_horz_separation;
+ // Distance between the center of the claw to the top of the claw.
+ // The line drawn at this distance parallel to the claw centerline is used
+ // to determine if claw interfears with the grabber.
+ double claw_top_thickness;
+ // The grabber is safe at any height if it is behind this location.
+ double grabber_always_safe_h_min;
+ // The grabber is safe at any x if it is above this location.
+ double grabber_always_safe_x_max;
+ };
+ ClawGeometry clawGeometry;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and