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.cc b/frc971/constants.cc
index 58066c6..04b9576 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -63,6 +63,43 @@
const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
const double kMaxAllowedLeftRightElevatorDifference = 0.01; // meters
+const Values::ClawGeometry kClawGeometry{
+ // Horizontal distance from the center of the grabber to the end.
+ 0.5 * 18.0 * 0.0254,
+ // Vertical distance from the arm rotation center to the bottom of
+ // the
+ // grabber. Distance measured with arm vertical (theta = 0).
+ 5.1 * 0.0254,
+ // Vertical separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ 10.5 * 0.0254,
+ // Horizontal separation of the claw and arm rotation centers with
+ // the
+ // elevator at 0.0 and the arm angle set to zero.
+ 6.5 * 0.0254,
+ // Distance between the center of the claw to the top of the claw.
+ // 2.75 inches would work most of the time. Using 3.5 inches because
+ // of the
+ // pnumatics fitting on the piston.
+ 3.5 * 0.0254,
+ // The grabber is safe at any height if it is behind this location.
+ // The location of the grabber is used here and not the location of
+ // the end
+ // of the grabber. The grabber location is (0, 0) when the elevator
+ // is at 0
+ // and the arm angle is 0.
+ (18.0 - 0.3) * 0.0254,
+ // The grabber is safe at any x if it is above this location.
+ // The location of the grabber is used here and not the location of
+ // the end
+ // of the grabber. The grabber location is (0, 0) when the elevator
+ // is at 0
+ // and the arm angle is 0.
+ // The "-5.4" is the location of the bottom of the grabber when
+ // the elevator is at the bottom (-0.3 inches) and arm angle is 0.
+ -8.0 * 0.0254,
+};
+
// Gearing ratios of the pots and encoders for the elevator and arm.
// Ratio is output shaft rotations per encoder/pot rotation
// Checked by Daniel on 2/13/15.
@@ -154,6 +191,7 @@
kMaxAllowedLeftRightArmDifference,
kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
};
break;
case kPracticeTeamNumber:
@@ -222,6 +260,7 @@
kMaxAllowedLeftRightArmDifference,
kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
};
break;
case kCompTeamNumber:
@@ -292,6 +331,7 @@
kMaxAllowedLeftRightArmDifference,
kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
};
break;
default:
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