Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/aos/common/byteorder.h b/aos/common/byteorder.h
index c616d68..ae6ecf4 100644
--- a/aos/common/byteorder.h
+++ b/aos/common/byteorder.h
@@ -5,6 +5,7 @@
#include <endian.h> // endian(3)
#endif
#include <string.h>
+#include <stdint.h>
// Contains functions for converting between host and network byte order for
// things other than 16/32 bit integers (those are provided by byteorder(3)).
diff --git a/aos/common/util/kinematics.h b/aos/common/util/kinematics.h
deleted file mode 100644
index 5718390..0000000
--- a/aos/common/util/kinematics.h
+++ /dev/null
@@ -1,388 +0,0 @@
-#ifndef AOS_COMMON_UTIL_KINEMATICS_H_
-#define AOS_COMMON_UTIL_KINEMATICS_H_
-
-#include <cmath>
-#include "Eigen/Dense"
-#include "frc971/constants.h"
-
-namespace aos {
-namespace util {
-
-// A class for performing forward and inverse kinematics on the elevator-arm
-// system. It can calculate where the fridge grabbers will be if the arm and
-// elevator are at a given position, as well as where the arm and elevator
-// should go in order to get the grabbers to a specific location.
-class ElevatorArmKinematics {
- public:
- typedef enum {
- // These specify the particular region that an invalid request was in. Right
- // is toward the front of the robot, left is toward the back.
-
- // Request is valid.
- REGION_VALID = 0,
- // Request is farther right than the arm can extend.
- REGION_RIGHT = 1 << 0,
- // Request is towards the front of the robot but higher than we can extend
- // with the elevator and the arm.
- REGION_UPPER_RIGHT = 1 << 1,
- // We can get the x part of the request, which is towards the front of the
- // robot, but not the h part, which is too high.
- REGION_INSIDE_UPPER_RIGHT = 1 << 2,
- // We can get the x part of the request, which is towards the front of the
- // robot, but not the h part, which is too low.
- REGION_INSIDE_LOWER_RIGHT = 1 << 3,
- // Request is towards the front of the robot but lower than we can extend
- // with the elevator and the arm.
- REGION_LOWER_RIGHT = 1 << 4,
- // Request is farther left than the arm can extend.
- REGION_LEFT = 1 << 5,
- // Request is towards the back of the robot but higher than we can extend
- // with the elevator and the arm.
- REGION_UPPER_LEFT = 1 << 6,
- // We can get the x part of the request, which is towards the front of the
- // robot, but not the h part, which is too high.
- REGION_INSIDE_UPPER_LEFT = 1 << 7,
- // We can get the x part of the request, which is towards the back of the
- // robot, but not the h part, which is too low.
- REGION_INSIDE_LOWER_LEFT = 1 << 8,
- // Request is towards the back of the robot but lower than we can extend
- // with the elevator and the arm.
- REGION_LOWER_LEFT = 1 << 9,
- // Request is invalid, but don't know where it is out of range.
- REGION_UNKNOWN = 1 << 10,
- } Region;
-
- class KinematicResult {
- public:
- // The elevator height result from an inverse kinematic.
- double elevator_height;
- // The arm angle result from an inverse kinematic.
- double arm_angle;
- // Resulting velocity of the elevator given x,y velocities.
- double elevator_velocity;
- // Resulting velocity of the arm given x,y velocities.
- double arm_velocity;
- // The fridge height value from a forward kinematic.
- double fridge_h;
- // The fridge x value from a forward kinematic.
- double fridge_x;
- // Resulting velocity of the fridge height given arm and elevator
- // velocities.
- double fridge_h_velocity;
- // Resulting velocity of the fridge x given arm and elevator velocities.
- double fridge_x_velocity;
- };
-
- // If we use the default constructor we wil just always not be able to move.
- ElevatorArmKinematics()
- : length_arm_(1.0),
- elevator_max_(0.0),
- elevator_min_(0.0),
- upper_angle_limit_(0.0),
- lower_angle_limit_(0.0) {}
-
- ElevatorArmKinematics(double length_arm, double height_max, double height_min,
- double angle_max, double angle_min)
- : length_arm_(length_arm),
- elevator_max_(height_max),
- elevator_min_(height_min),
- upper_angle_limit_(angle_max),
- lower_angle_limit_(angle_min),
- geometry_(frc971::constants::GetValues().clawGeometry) {}
-
- ~ElevatorArmKinematics() {}
-
- // Limit a number to the speed of light. The loops should handle this a lot
- // better than overflow.
- void LimitLightSpeed(double* num) {
- if (*num > 299792458.0) {
- *num = 299792458.0;
- }
- if (*num < -299792458.0) {
- *num = -299792458.0;
- }
- if (!::std::isfinite(*num)) {
- *num = 0.0;
- }
- }
-
- // Calculates the arm angle in radians and the elevator height in meters for
- // a desired Fridge grabber height and x location. x is positive going
- // toward the front of the robot.
- // h is positive going up. x=0 and h=0 is the location of the top fridge
- // grabber when the elevator is at 0 height and the arm angle is 0 (vertical).
- // Both the x and h values are given in meters.
- // Returns the region of the request.
- // Result is:
- // the angle of the arm in radians
- // the height of the elevator in meters
- // the resulting x
- // and the resulting h
- // If an impossible location is requested, the arm angle and elevator height
- // returned are the closest possible for the requested fridge grabber height.
- // If the requested height is above the max possible height, the angle
- // will be 0 and the height will be the max possible height of the elevator.
- int InverseKinematic(double request_x, double request_h,
- double request_x_velocity, double request_y_velocity,
- KinematicResult* result) {
- int valid_or_invalid = REGION_VALID;
-
- double square_arm = length_arm_ * length_arm_;
- double term = ::std::sqrt(square_arm - request_x * request_x);
-
- // Check to see if the x location can be satisfied. If the requested x
- // location
- // is further out than the arm can go, it is not possible for any elevator
- // location.
- if (request_x > length_arm_) {
- result->arm_angle = -M_PI * 0.5;
- valid_or_invalid |= REGION_RIGHT;
- } else if (request_x < -length_arm_) {
- result->arm_angle = M_PI * 0.5;
- valid_or_invalid |= REGION_LEFT;
- } else {
- result->arm_angle = ::std::asin(-request_x / length_arm_);
- result->arm_velocity = (-1.0 / term) * request_x_velocity;
- LimitLightSpeed(&result->arm_velocity);
- }
-
- result->elevator_height =
- request_h + length_arm_ * (1.0 - ::std::cos(result->arm_angle));
- result->elevator_velocity =
- (request_x / (square_arm * term)) * request_x_velocity +
- request_y_velocity;
- LimitLightSpeed(&result->elevator_velocity);
-
- // Check to see if the requested elevator height is possible
- if (request_h > elevator_max_) {
- // The elevator cannot go high enough with any arm angle to satisfy this
- // request. So position the elevator at the top and the arm angle set to
- // vertical.
- result->elevator_height = elevator_max_;
- result->arm_angle = 0.0;
- if (request_x >= 0) {
- valid_or_invalid |= REGION_UPPER_RIGHT;
- } else {
- valid_or_invalid |= REGION_UPPER_LEFT;
- }
- } else if (request_h < -length_arm_ + elevator_min_) {
- // The elevator cannot go low enough with any arm angle to satisfy this
- // request. So position the elevator at the bottom and the arm angle to
- // satisfy the x request The elevator will move up as the grabber moves to
- // the center of the robot when in this part of the motion space.
- result->elevator_height = elevator_min_;
- if (request_x >= 0) {
- valid_or_invalid |= REGION_LOWER_RIGHT;
- } else {
- valid_or_invalid |= REGION_LOWER_LEFT;
- }
- } else if (result->elevator_height > elevator_max_) {
- // Impossibly high request. So get as close to the x request with the
- // elevator at the top of its range.
- result->elevator_height = elevator_max_;
- if (request_x >= 0) {
- result->arm_angle =
- -::std::acos((length_arm_ + request_h - elevator_max_) /
- length_arm_);
- valid_or_invalid |= REGION_INSIDE_UPPER_RIGHT;
- } else {
- result->arm_angle = ::std::acos(
- (length_arm_ + request_h - elevator_max_) / length_arm_);
- valid_or_invalid |= REGION_INSIDE_UPPER_LEFT;
- }
- } else if (result->elevator_height < elevator_min_) {
- // Impossibly low request. So satisfy the x request with the elevator
- // at the bottom of its range.
- // The elevator will move up as the grabber moves to the center of the
- // robot
- // when in this part of the motion space.
- result->elevator_height = elevator_min_;
- if (request_x >= 0) {
- valid_or_invalid |= REGION_INSIDE_LOWER_RIGHT;
- } else {
- valid_or_invalid |= REGION_INSIDE_LOWER_LEFT;
- }
- }
-
- // if we are not in a valid region we will zero the velocity for now
- if (valid_or_invalid != REGION_VALID) {
- result->arm_velocity = 0.0;
- result->elevator_velocity = 0.0;
- }
-
- if (ForwardKinematic(result->elevator_height, result->arm_angle,
- result->elevator_velocity, result->arm_velocity,
- result) == REGION_UNKNOWN) {
- return REGION_UNKNOWN;
- }
- return valid_or_invalid;
- }
-
- // Takes an elevator height and arm angle and projects these to the resulting
- // fridge height and x offset. Returns REGION_UNKNOWN if the values are
- // outside
- // limits. This will result in the height/angle being bounded and the
- // resulting position will be returned.
- Region ForwardKinematic(double elevator_height, double arm_angle,
- double elevator_velocity, double arm_velocity,
- KinematicResult* result) {
- result->elevator_height = elevator_height;
- result->arm_angle = arm_angle;
-
- Region valid = REGION_VALID;
- if (elevator_height < elevator_min_) {
- LOG(WARNING, "elevator %.2f limited at %.2f\n", result->elevator_height,
- elevator_min_);
- result->elevator_height = elevator_min_;
- valid = REGION_UNKNOWN;
- }
- if (elevator_height > elevator_max_) {
- LOG(WARNING, "elevator %.2f limited at %.2f\n", result->elevator_height,
- elevator_max_);
- result->elevator_height = elevator_max_;
- valid = REGION_UNKNOWN;
- }
- if (arm_angle < lower_angle_limit_) {
- LOG(WARNING, "arm %.2f limited at %.2f\n", result->arm_angle,
- lower_angle_limit_);
- result->arm_angle = lower_angle_limit_;
- valid = REGION_UNKNOWN;
- }
- if (arm_angle > upper_angle_limit_) {
- result->arm_angle = upper_angle_limit_;
- LOG(WARNING, "arm %.2f limited at %.2f\n", result->arm_angle,
- upper_angle_limit_);
- valid = REGION_UNKNOWN;
- }
- // Compute the fridge grabber height and x location using the computed
- // elevator height and arm angle.
- result->fridge_h = result->elevator_height +
- (::std::cos(result->arm_angle) - 1.0) * length_arm_;
- result->fridge_x = -::std::sin(result->arm_angle) * length_arm_;
- // velocity based on joacobian
- result->fridge_x_velocity =
- -length_arm_ * ::std::cos(result->arm_angle) * arm_velocity;
- LimitLightSpeed(&result->fridge_x_velocity);
- result->fridge_h_velocity =
- -length_arm_ * ::std::sin(result->arm_angle) * arm_velocity +
- elevator_velocity;
- LimitLightSpeed(&result->fridge_h_velocity);
- return valid;
- }
-
- // Same as ForwardKinematic but without any checking.
- Eigen::Vector2d ForwardKinematicNoChecking(double elevator_height,
- double arm_angle) {
- // Compute the fridge grabber height and x location using the computed
- // elevator height and arm angle.
- Eigen::Vector2d grabber_location;
- grabber_location.y() =
- elevator_height + (::std::cos(arm_angle) - 1.0) * length_arm_;
- grabber_location.x() = -::std::sin(arm_angle) * length_arm_;
- return grabber_location;
- }
-
- // 2 dimensional version of cross product
- double Cross(Eigen::Vector2d a, Eigen::Vector2d b) {
- double crossProduct = a.x() * b.y() - a.y() * b.x();
- return crossProduct;
- }
-
- // Tell whether or not it is safe to move the grabber to a position.
- // Returns true if the current move is safe.
- // If it returns false then a safe_claw_angle that is greater than zero is
- // acceptable otherwise if safe_claw_angle is less than zero there will be no
- // valid solution.
- bool GrabberArmIntersectionCheck(double elevator_height, double arm_angle,
- double claw_angle, double* safe_claw_angle) {
- Eigen::Vector2d grabber_location =
- ForwardKinematicNoChecking(elevator_height, arm_angle);
- if (grabber_location.x() < geometry_.grabber_always_safe_x_max ||
- grabber_location.y() > geometry_.grabber_always_safe_h_min) {
- *safe_claw_angle = claw_angle;
- return true;
- }
- Eigen::Vector2d grabber_bottom_end;
- Eigen::Vector2d claw_i_unit_direction(::std::cos(claw_angle),
- sin(claw_angle));
- Eigen::Vector2d claw_j_unit_direction(-::std::sin(claw_angle),
- cos(claw_angle));
-
- // Vector from the center of the arm rotation axis to front bottom
- // corner of the grabber.
- Eigen::Vector2d grabber_end_location_from_arm_axis(
- geometry_.grabber_half_length, -geometry_.grabber_delta_y);
-
- // Bottom front corner of the grabber. This is what will usually hit the
- // claw first.
- grabber_bottom_end = grabber_location + grabber_end_location_from_arm_axis;
-
- // Location of the claw horizontal axis of rotation relative to the
- // arm axis of rotation with the elevator at 0 and the arm angle of 0
- // The horizontal axis is the up and down motion axis.
- Eigen::Vector2d claw_updown_axis(geometry_.grabber_arm_horz_separation,
- -geometry_.grabber_arm_vert_separation);
-
- // This point is used to make a cross product with the bottom end of the
- // grabber
- // The result of the cross product tells if the parts intersect or not.
- Eigen::Vector2d claw_top_ref_point =
- claw_updown_axis + geometry_.claw_top_thickness * claw_j_unit_direction;
-
- Eigen::Vector2d claw_top_ref_point_to_grabber_bottom_end =
- grabber_bottom_end - claw_top_ref_point;
- double claw_grabber_check =
- Cross(claw_i_unit_direction, claw_top_ref_point_to_grabber_bottom_end);
-
- // Now set the safe claw angle.
- if (claw_grabber_check > 0.0) {
- *safe_claw_angle = claw_angle;
- return true;
- } else if (grabber_bottom_end.y() <
- claw_updown_axis.y() +
- geometry_.claw_top_thickness) { // grabber is too close
- *safe_claw_angle = -1.0;
- return false;
- } else {
- // To find the safe angle for the claw, draw a line between the claw
- // rotation axis and the lower front corner of the grabber. The angle of
- // this line is used with the angle between the edge of the claw and the
- // center line of the claw to determine the angle of the claw.
- Eigen::Vector2d claw_axis_to_grabber_bottom_end =
- grabber_bottom_end - claw_updown_axis;
- double hypot = claw_axis_to_grabber_bottom_end.norm();
- double angleDiff = ::std::asin(geometry_.claw_top_thickness / hypot);
- *safe_claw_angle = ::std::atan2(claw_axis_to_grabber_bottom_end.y(),
- claw_axis_to_grabber_bottom_end.x()) -
- angleDiff;
- return false;
- }
- }
-
- double get_elevator_min() { return elevator_min_; }
-
- double get_elevator_max() { return elevator_max_; }
-
- double get_upper_angle_limit() { return upper_angle_limit_; }
-
- double get_lower_angle_limit() { return lower_angle_limit_; }
-
- private:
- // length of the arm
- double length_arm_;
- // max height the elevator can go.
- double elevator_max_;
- // min height the elevator can go.
- double elevator_min_;
- // arm angle upper limit
- double upper_angle_limit_;
- // arm angle lower limit
- double lower_angle_limit_;
- // Geometry of the arm + fridge
- frc971::constants::Values::ClawGeometry geometry_;
-};
-
-} // namespace util
-} // namespace aos
-
-#endif // AOS_COMMON_UTIL_KINEMATICS_H_
diff --git a/aos/common/util/kinematics_test.cc b/aos/common/util/kinematics_test.cc
deleted file mode 100644
index 268f78d..0000000
--- a/aos/common/util/kinematics_test.cc
+++ /dev/null
@@ -1,242 +0,0 @@
-#include <cmath>
-
-#include "gtest/gtest.h"
-
-#include "aos/common/logging/logging.h"
-#include "aos/common/queue_testutils.h"
-#include "aos/common/util/kinematics.h"
-#include "frc971/control_loops/team_number_test_environment.h"
-
-namespace aos {
-namespace util {
-namespace testing {
-
-// For verifcation against Mr. Schuh's internal tests.
-// Please do not comment on this in a code review. We can remove this after the
-// season to satisfy any OCD.
-bool k_internal_debug = false;
-
-class KinematicsTest : public ::testing::Test {
- public:
- KinematicsTest()
- : lower_height_limit_(0.01),
- upper_height_limit_(0.65),
- lower_angle_limit_(-M_PI / 2.0),
- upper_angle_limit_(M_PI / 2.0) {}
-
- void SetUp() {
- ::aos::common::testing::EnableTestLogging();
- kinematics_ = ElevatorArmKinematics(arm_length_, upper_height_limit_, lower_height_limit_,
- upper_angle_limit_, lower_angle_limit_);
- }
-
- protected:
- double lower_height_limit_;
- double upper_height_limit_;
- double lower_angle_limit_;
- double upper_angle_limit_;
- double arm_length_ = 0.7366;
-
- ElevatorArmKinematics kinematics_;
-};
-
-// Used for internal debugging and verification only not acctual testing.
-// Please do not comment on this in a code review. We can remove this after
-// the season to satisfy any OCD.
-TEST_F(KinematicsTest,
- PrintPointsSoWeCanHandVerifyThemUntilABetterTestIsMadeAndBrianIsHappy) {
- if (k_internal_debug) {
- for (double y = -1.0; y <= 1.0; y += 0.2) {
- for (double x = -1.0; x <= 1.0; x += 0.2) {
- ElevatorArmKinematics::KinematicResult res;
- int region = kinematics_.InverseKinematic(x, y, 0.0, 0.0, &res);
- printf(
- " %12.3f %12.3f %8.3f %9.3f %8.2f %12d %12.4f %10.4f "
- "%15.4f %16.4f\n",
- x, y, res.elevator_height, res.arm_angle,
- res.arm_angle * 180.0 / M_PI, region, res.fridge_x, res.fridge_h,
- res.fridge_x - x, res.fridge_h - y);
- }
- }
-
- // Make a set of calls to test the grabber arm intersection test code.
- printf(
- "# ArmAngle (degrees) ElevatorHeight ClawAngle (degrees) GrabberX "
- "GrabberH intersectReturn SafeClawAngle (degrees)\n");
- for (double elevator_height = kinematics_.get_elevator_min();
- elevator_height <= kinematics_.get_elevator_max();
- elevator_height += 0.10) {
- for (double arm_angle = kinematics_.get_lower_angle_limit();
- arm_angle <= kinematics_.get_upper_angle_limit() + 0.01;
- arm_angle += M_PI * 0.5 / 9.0) {
- double claw_angle = M_PI * 0.25;
- double safe_claw_angle;
- double intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- Eigen::Vector2d grabber_location =
- kinematics_.ForwardKinematicNoChecking(elevator_height, arm_angle);
-
- printf(
- " %8.4f %8.2f %8.2f %14.4f %9.2f %9.2f %9.2f %10.3f %13.4f "
- "%12.3f\n",
- arm_angle, arm_angle * 180.0 / M_PI, elevator_height, claw_angle,
- claw_angle * 180.0 / M_PI, grabber_location.x(),
- grabber_location.y(), intersectReturnValue, safe_claw_angle,
- safe_claw_angle * 180.0 / M_PI);
- }
- printf("\n");
- }
- }
-}
-
-TEST_F(KinematicsTest, ValidIntersectCheckPointAtBottomOfElevatorRange) {
- double safe_claw_angle;
- double elevator_height = 0.01;
- double arm_angle = 30.0 * M_PI / 180.0;
- double claw_angle = M_PI * 0.25;
- bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- EXPECT_TRUE(intersectReturnValue);
- EXPECT_EQ(safe_claw_angle, claw_angle);
-}
-
-TEST_F(KinematicsTest, ValidIntersectCheckPointAtMiddleOfElevatorRange) {
- double safe_claw_angle;
- double elevator_height = 0.4;
- double arm_angle = 30.0 * M_PI / 180.0;
- double claw_angle = M_PI * 0.25;
- bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- EXPECT_TRUE(intersectReturnValue);
- EXPECT_EQ(safe_claw_angle, claw_angle);
-}
-
-TEST_F(KinematicsTest,
- invalidIntersectCheckPointAtBottomOfElevatorRangeWithSafeClawAngle) {
- double safe_claw_angle;
- double elevator_height = 0.01;
- double arm_angle = -20.0 * M_PI / 180.0;
- double claw_angle = M_PI * 0.25;
- bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- EXPECT_FALSE(intersectReturnValue);
- EXPECT_NEAR(safe_claw_angle, 0.0435733, 0.000001);
-}
-
-TEST_F(KinematicsTest,
- invalidIntersectCheckPointAtMiddleOfElevatorRangeWithSafeClawAngle) {
- double safe_claw_angle;
- double elevator_height = 0.41;
- double arm_angle = -60.0 * M_PI / 180.0;
- double claw_angle = M_PI * 0.25;
- bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- EXPECT_FALSE(intersectReturnValue);
- EXPECT_NEAR(safe_claw_angle, 0.12655341, 0.000001);
-}
-
-TEST_F(KinematicsTest,
- invalidIntersectCheckPointAtBottomOfElevatorRangeNoSafeClawAngle) {
- double safe_claw_angle;
- double elevator_height = 0.01;
- double arm_angle = -30.0 * M_PI / 180.0;
- double claw_angle = M_PI * 0.25;
- bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- EXPECT_FALSE(intersectReturnValue);
- EXPECT_EQ(safe_claw_angle, -1.0);
-}
-
-TEST_F(KinematicsTest,
- invalidIntersectCheckPointAtMiddleOfElevatorRangeNoSafeClawAngle) {
- double safe_claw_angle;
- double elevator_height = 0.41;
- double arm_angle = -70.0 * M_PI / 180.0;
- double claw_angle = M_PI * 0.25;
- bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
- elevator_height, arm_angle, claw_angle, &safe_claw_angle);
- EXPECT_FALSE(intersectReturnValue);
- EXPECT_EQ(safe_claw_angle, -1.0);
-}
-
-// Tests that velocity calulations are correct
-TEST_F(KinematicsTest, InverseKinematicVelocity) {
- ElevatorArmKinematics::KinematicResult result;
- // move striaght up and verify that only hieght changes
- EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, 0.0, 0.7, &result));
- EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
- EXPECT_NEAR(0.7, result.elevator_velocity, 0.00001);
- // check the negative
- EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, 0.0, -0.7, &result));
- EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
- EXPECT_NEAR(-0.7, result.elevator_velocity, 0.00001);
- // even with the arm out we should still just move up
- EXPECT_EQ(0, kinematics_.InverseKinematic(M_PI / 6, 0.2, 0.0, 0.7, &result));
- EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
- EXPECT_NEAR(0.7, result.elevator_velocity, 0.00001);
- // even with the arm back we should still just move up
- EXPECT_EQ(0, kinematics_.InverseKinematic(-M_PI / 6, 0.2, 0.0, 0.7, &result));
- EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
- EXPECT_NEAR(0.7, result.elevator_velocity, 0.00001);
-
- // should move only angle forward
- EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, 1.0, 0.0, &result));
- EXPECT_NEAR(-1.35759, result.arm_velocity, 0.00001);
- EXPECT_NEAR(0.0, result.elevator_velocity, 0.00001);
- // check the negative
- EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, -1.0, 0.0, &result));
- EXPECT_NEAR(1.35759, result.arm_velocity, 0.00001);
- EXPECT_NEAR(0.0, result.elevator_velocity, 0.00001);
- // with the arm out a change in x should make arm angle greater and
- // bring the evevator down.
- EXPECT_EQ(0, kinematics_.InverseKinematic(0.2, 0.2, 1.0, 0.0, &result));
- EXPECT_GT(0.0, result.arm_velocity);
- EXPECT_LT(0.0, result.elevator_velocity);
- // with the arm out a change in x should make arm angle greater and
- // bring the evevator down.
- EXPECT_EQ(0, kinematics_.InverseKinematic(-0.2, 0.2, 1.0, 0.0, &result));
- EXPECT_GT(0.0, result.arm_velocity);
- EXPECT_GT(0.0, result.elevator_velocity);
-}
-
-// Tests that velocity calulations are correct
-TEST_F(KinematicsTest, ForwardKinematicVelocity) {
- ElevatorArmKinematics::KinematicResult result;
-
- // moving the arm forward at zero should result in x velocity
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, 0.0, 1.35759, &result));
- EXPECT_NEAR(-1.0, result.fridge_x_velocity, 0.00001);
- EXPECT_NEAR(0.0, result.fridge_h_velocity, 0.00001);
- // check the negative
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, 0.0, -1.35759, &result));
- EXPECT_NEAR(1.0, result.fridge_x_velocity, 0.00001);
- EXPECT_NEAR(0.0, result.fridge_h_velocity, 0.00001);
- // moving the arm up at zero should result in h velocity
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, 1.0, 0.0, &result));
- EXPECT_NEAR(0.0, result.fridge_x_velocity, 0.00001);
- EXPECT_NEAR(1.0, result.fridge_h_velocity, 0.00001);
- // check the negative
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, -1.0, 0.0, &result));
- EXPECT_NEAR(0.0, result.fridge_x_velocity, 0.00001);
- EXPECT_NEAR(-1.0, result.fridge_h_velocity, 0.00001);
- // arm is forward a negative angle should make x head forward and y head down.
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, -0.2, 0.0, -1.0, &result));
- EXPECT_GT(result.fridge_x_velocity, 0.0);
- EXPECT_LT(result.fridge_h_velocity, 0.0);
- // arm is forward a positive angle should make x head backwardward and y head up.
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, -0.2, 0.0, 1.0, &result));
- EXPECT_LT(result.fridge_x_velocity, 0.0);
- EXPECT_GT(result.fridge_h_velocity, 0.0);
- // arm is backward a negative angle should make x head forward and y head down.
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, 0.2, 0.0, -1.0, &result));
- EXPECT_GT(result.fridge_x_velocity, 0.0);
- EXPECT_GT(result.fridge_h_velocity, 0.0);
- // arm is backward a negative angle should make x head forward and y head down.
- EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, 0.2, 0.0, 1.0, &result));
- EXPECT_LT(result.fridge_x_velocity, 0.0);
- EXPECT_LT(result.fridge_h_velocity, 0.0);
-}
-
-} // namespace testing
-} // namespace util
-} // namespace aos
diff --git a/aos/common/util/util.gyp b/aos/common/util/util.gyp
index f60622e..e9a37c9 100644
--- a/aos/common/util/util.gyp
+++ b/aos/common/util/util.gyp
@@ -149,34 +149,5 @@
'<(EXTERNALS):gtest',
],
},
- {
- 'target_name': 'kinematics',
- 'type': 'static_library',
- 'sources': [
- #'kinematics.h',
- ],
- 'dependencies': [
- '<(EXTERNALS):eigen',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- ],
- 'export_dependent_settings': [
- '<(EXTERNALS):eigen',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- ],
- },
- {
- 'target_name': 'kinematics_test',
- 'type': 'executable',
- 'sources': [
- 'kinematics_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
- 'kinematics'
- ],
- },
],
}