diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp new file mode 100644 index 0000000000..8301481146 --- /dev/null +++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +using namespace frc; + +DifferentialDriveKinematics::DifferentialDriveKinematics( + units::meter_t trackWidth) + : m_trackWidth(trackWidth) {} + +ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds( + const DifferentialDriveWheelSpeeds& wheelSpeeds) const { + return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps, + (wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad}; +} + +DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const { + return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad, + chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad}; +} diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp new file mode 100644 index 0000000000..418dd0f5ad --- /dev/null +++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/DifferentialDriveOdometry.h" + +using namespace frc; + +DifferentialDriveOdometry::DifferentialDriveOdometry( + DifferentialDriveKinematics kinematics, const Pose2d& initialPose) + : m_kinematics(kinematics), m_pose(initialPose) { + m_previousAngle = m_pose.Rotation(); +} + +const Pose2d& DifferentialDriveOdometry::UpdateWithTime( + units::second_t currentTime, const Rotation2d& angle, + const DifferentialDriveWheelSpeeds& wheelSpeeds) { + units::second_t deltaTime = + (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; + m_previousTime = currentTime; + + auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds); + static_cast(dtheta); + + auto newPose = m_pose.Exp( + {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()}); + + m_previousAngle = angle; + m_pose = {newPose.Translation(), angle}; + + return m_pose; +} diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp new file mode 100644 index 0000000000..5b13f0ecad --- /dev/null +++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +using namespace frc; + +void DifferentialDriveWheelSpeeds::Normalize( + units::meters_per_second_t attainableMaxSpeed) { + auto realMaxSpeed = + units::math::max(units::math::abs(left), units::math::abs(right)); + + if (realMaxSpeed > attainableMaxSpeed) { + left = left / realMaxSpeed * attainableMaxSpeed; + right = right / realMaxSpeed * attainableMaxSpeed; + } +} diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp new file mode 100644 index 0000000000..bbe06470cb --- /dev/null +++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/MecanumDriveKinematics.h" + +using namespace frc; + +MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) { + // We have a new center of rotation. We need to compute the matrix again. + if (centerOfRotation.X() != m_previousCoR.X() || + centerOfRotation.Y() != m_previousCoR.Y()) { + auto fl = m_frontLeftWheel - centerOfRotation; + auto fr = m_frontRightWheel - centerOfRotation; + auto rl = m_rearLeftWheel - centerOfRotation; + auto rr = m_rearRightWheel - centerOfRotation; + + SetInverseKinematics(fl, fr, rl, rr); + + m_previousCoR = centerOfRotation; + } + + Eigen::Vector3d chassisSpeedsVector; + chassisSpeedsVector << chassisSpeeds.vx.to(), + chassisSpeeds.vy.to(), chassisSpeeds.omega.to(); + + Eigen::Matrix wheelsMatrix = + m_inverseKinematics * chassisSpeedsVector; + + MecanumDriveWheelSpeeds wheelSpeeds; + wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)}; + wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)}; + wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)}; + wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)}; + return wheelSpeeds; +} + +ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds( + const MecanumDriveWheelSpeeds& wheelSpeeds) { + Eigen::Matrix wheelSpeedsMatrix; + // clang-format off + wheelSpeedsMatrix << wheelSpeeds.frontLeft.to(), wheelSpeeds.frontRight.to(), + wheelSpeeds.rearLeft.to(), wheelSpeeds.rearRight.to(); + // clang-format on + + Eigen::Vector3d chassisSpeedsVector = + m_forwardKinematics.solve(wheelSpeedsMatrix); + + return {units::meters_per_second_t{chassisSpeedsVector(0)}, + units::meters_per_second_t{chassisSpeedsVector(1)}, + units::radians_per_second_t{chassisSpeedsVector(2)}}; +} + +void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, + Translation2d fr, + Translation2d rl, + Translation2d rr) { + // clang-format off + m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to(), + 1, 1, (fr.X() - fr.Y()).template to(), + 1, 1, (rl.X() - rl.Y()).template to(), + 1, -1, (-(rr.X() + rr.Y())).template to(); + // clang-format on + m_inverseKinematics /= std::sqrt(2); +} diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp new file mode 100644 index 0000000000..18e563527d --- /dev/null +++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/MecanumDriveOdometry.h" + +using namespace frc; + +MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics, + const Pose2d& initialPose) + : m_kinematics(kinematics), m_pose(initialPose) { + m_previousAngle = m_pose.Rotation(); +} + +const Pose2d& MecanumDriveOdometry::UpdateWithTime( + units::second_t currentTime, const Rotation2d& angle, + MecanumDriveWheelSpeeds wheelSpeeds) { + units::second_t deltaTime = + (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; + m_previousTime = currentTime; + + auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds); + static_cast(dtheta); + + auto newPose = m_pose.Exp( + {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()}); + + m_previousAngle = angle; + m_pose = {newPose.Translation(), angle}; + + return m_pose; +} diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp new file mode 100644 index 0000000000..1641ba87ef --- /dev/null +++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +#include +#include +#include + +using namespace frc; + +void MecanumDriveWheelSpeeds::Normalize( + units::meters_per_second_t attainableMaxSpeed) { + std::array wheelSpeeds{frontLeft, frontRight, + rearLeft, rearRight}; + units::meters_per_second_t realMaxSpeed = *std::max_element( + wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) { + return units::math::abs(a) < units::math::abs(b); + }); + + if (realMaxSpeed > attainableMaxSpeed) { + for (int i = 0; i < 4; ++i) { + wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed; + } + frontLeft = wheelSpeeds[0]; + frontRight = wheelSpeeds[1]; + rearLeft = wheelSpeeds[2]; + rearRight = wheelSpeeds[3]; + } +} diff --git a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h new file mode 100644 index 0000000000..8c772c0bb9 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/geometry/Rotation2d.h" + +namespace frc { +/** + * Represents the speed of a robot chassis. Although this struct contains + * similar members compared to a Twist2d, they do NOT represent the same thing. + * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of + * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot + * frame of reference. + * + * A strictly non-holonomic drivetrain, such as a differential drive, should + * never have a dy component because it can never move sideways. Holonomic + * drivetrains such as swerve and mecanum will often have all three components. + */ +struct ChassisSpeeds { + /** + * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) + */ + units::meters_per_second_t vx = 0_mps; + + /** + * Represents strafe velocity w.r.t the robot frame of reference. (Left is +) + */ + units::meters_per_second_t vy = 0_mps; + + /** + * Represents the angular velocity of the robot frame. (CCW is +) + */ + units::radians_per_second_t omega = 0_rad_per_s; + + /** + * Converts a user provided field-relative set of speeds into a robot-relative + * ChassisSpeeds object. + * + * @param vx The component of speed in the x direction relative to the field. + * Positive x is away from your alliance wall. + * @param vy The component of speed in the y direction relative to the field. + * Positive y is to your left when standing behind the alliance wall. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away from + * your alliance station wall. Remember that this should be CCW positive. + * + * @return ChassisSpeeds object representing the speeds in the robot's frame + * of reference. + */ + static ChassisSpeeds FromFieldRelativeSpeeds( + units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, const Rotation2d& robotAngle) { + return {vx * robotAngle.Cos() + vy * robotAngle.Sin(), + -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega}; + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h new file mode 100644 index 0000000000..34407b94df --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +namespace frc { +/** + * Helper class that converts a chassis velocity (dx and dtheta components) to + * left and right wheel velocities for a differential drive. + * + * Inverse kinematics converts a desired chassis speed into left and right + * velocity components whereas forward kinematics converts left and right + * component velocities into a linear and angular chassis speed. + */ +class DifferentialDriveKinematics { + public: + /** + * Constructs a differential drive kinematics object. + * + * @param trackWidth The track width of the drivetrain. Theoretically, this is + * the distance between the left wheels and right wheels. However, the + * empirical value may be larger than the physical measured value due to + * scrubbing effects. + */ + explicit DifferentialDriveKinematics(units::meter_t trackWidth); + + /** + * Returns a chassis speed from left and right component velocities using + * forward kinematics. + * + * @param wheelSpeeds The left and right velocities. + * @return The chassis speed. + */ + ChassisSpeeds ToChassisSpeeds( + const DifferentialDriveWheelSpeeds& wheelSpeeds) const; + + /** + * Returns left and right component velocities from a chassis speed using + * inverse kinematics. + * + * @param chassisSpeeds The linear and angular (dx and dtheta) components that + * represent the chassis' speed. + * @return The left and right velocities. + */ + DifferentialDriveWheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const; + + private: + units::meter_t m_trackWidth; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h new file mode 100644 index 0000000000..7e463426e1 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "DifferentialDriveKinematics.h" +#include "frc/geometry/Pose2d.h" + +namespace frc { +/** + * Class for differential drive odometry. Odometry allows you to track the + * robot's position on the field over the course of a match using readings from + * 2 encoders and a gyroscope. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + * + * Note: It is important to reset both your encoders to zero before you start + * using this class. Only reset your encoders ONCE. You should not reset your + * encoders even if you want to reset your robot's pose. + */ +class DifferentialDriveOdometry { + public: + /** + * Constructs a DifferentialDriveOdometry object. + * + * @param kinematics The differential drive kinematics for your drivetrain. + * @param initialPose The starting position of the robot on the field. + */ + explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, + const Pose2d& initialPose = Pose2d()); + + /** + * Resets the robot's position on the field. + * + * @param pose The position on the field that your robot is at. + */ + void ResetPosition(const Pose2d& pose) { + m_pose = pose; + m_previousAngle = pose.Rotation(); + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param currentTime The current time. + * @param angle The angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * + * @return The new pose of the robot. + */ + const Pose2d& UpdateWithTime(units::second_t currentTime, + const Rotation2d& angle, + const DifferentialDriveWheelSpeeds& wheelSpeeds); + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method automatically calculates + * the current time to calculate period (difference between two timestamps). + * The period is used to calculate the change in distance from a velocity. + * This also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param angle The angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * + * @return The new pose of the robot. + */ + const Pose2d& Update(const Rotation2d& angle, + const DifferentialDriveWheelSpeeds& wheelSpeeds) { + const auto now = std::chrono::system_clock::now().time_since_epoch(); + units::second_t time{now}; + return UpdateWithTime(time, angle, wheelSpeeds); + } + + private: + DifferentialDriveKinematics m_kinematics; + Pose2d m_pose; + + units::second_t m_previousTime = -1_s; + Rotation2d m_previousAngle; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h new file mode 100644 index 0000000000..66bd84e235 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +namespace frc { +/** + * Represents the wheel speeds for a differential drive drivetrain. + */ +struct DifferentialDriveWheelSpeeds { + /** + * Speed of the left side of the robot. + */ + units::meters_per_second_t left = 0_mps; + + /** + * Speed of the right side of the robot. + */ + units::meters_per_second_t right = 0_mps; + + /** + * Normalizes the wheel speeds using some max attainable speed. Sometimes, + * after inverse kinematics, the requested speed from a/several modules may be + * above the max attainable speed for the driving motor on that module. To fix + * this issue, one can "normalize" all the wheel speeds to make sure that all + * requested module speeds are below the absolute threshold, while maintaining + * the ratio of speeds between modules. + * + * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + */ + void Normalize(units::meters_per_second_t attainableMaxSpeed); +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h new file mode 100644 index 0000000000..6b0329de00 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -0,0 +1,141 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "frc/geometry/Translation2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +namespace frc { + +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual wheel speeds. + * + * The inverse kinematics (converting from a desired chassis velocity to + * individual wheel speeds) uses the relative locations of the wheels with + * respect to the center of rotation. The center of rotation for inverse + * kinematics is also variable. This means that you can set your set your center + * of rotation in a corner of the robot to perform special evasion manuevers. + * + * Forward kinematics (converting an array of wheel speeds into the overall + * chassis motion) is performs the exact opposite of what inverse kinematics + * does. Since this is an overdetermined system (more equations than variables), + * we use a least-squares approximation. + * + * The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] + * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then + * multiply by [wheelSpeeds] to get our chassis speeds. + * + * Forward kinematics is also used for odometry -- determining the position of + * the robot on the field using encoders and a gyro. + */ +class MecanumDriveKinematics { + public: + /** + * Constructs a mecanum drive kinematics object. + * + * @param frontLeftWheel The location of the front-left wheel relative to the + * physical center of the robot. + * @param frontRightWheel The location of the front-right wheel relative to + * the physical center of the robot. + * @param rearLeftWheel The location of the rear-left wheel relative to the + * physical center of the robot. + * @param rearRightWheel The location of the rear-right wheel relative to the + * physical center of the robot. + */ + explicit MecanumDriveKinematics(Translation2d frontLeftWheel, + Translation2d frontRightWheel, + Translation2d rearLeftWheel, + Translation2d rearRightWheel) + : m_frontLeftWheel{frontLeftWheel}, + m_frontRightWheel{frontRightWheel}, + m_rearLeftWheel{rearLeftWheel}, + m_rearRightWheel{rearRightWheel} { + SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel, + rearRightWheel); + m_forwardKinematics = m_inverseKinematics.householderQr(); + } + + MecanumDriveKinematics(const MecanumDriveKinematics&) = default; + + /** + * Performs inverse kinematics to return the wheel speeds from a desired + * chassis velocity. This method is often used to convert joystick values into + * wheel speeds. + * + * This function also supports variable centers of rotation. During normal + * operations, the center of rotation is usually the same as the physical + * center of the robot; therefore, the argument is defaulted to that use case. + * However, if you wish to change the center of rotation for evasive + * manuevers, vision alignment, or for any other use case, you can do so. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotation The center of rotation. For example, if you set the + * center of rotation at one corner of the robot and + * provide a chassis speed that only has a dtheta + * component, the robot will rotate around that + * corner. + * + * @return The wheel speeds. Use caution because they are not normalized. + * Sometimes, a user input may cause one of the wheel speeds to go + * above the attainable max velocity. Use the + * MecanumDriveWheelSpeeds::Normalize() function to rectify this + * issue. In addition, you can leverage the power of C++17 to directly + * assign the wheel speeds to variables: + * + * @code{.cpp} + * auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds); + * @endcode + */ + MecanumDriveWheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds, + const Translation2d& centerOfRotation = Translation2d()); + + /** + * Performs forward kinematics to return the resulting chassis state from the + * given wheel speeds. This method is often used for odometry -- determining + * the robot's position on the field using data from the real-world speed of + * each wheel on the robot. + * + * @param wheelSpeeds The current mecanum drive wheel speeds. + * + * @return The resulting chassis speed. + */ + ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds); + + private: + Eigen::Matrix m_inverseKinematics; + Eigen::HouseholderQR> m_forwardKinematics; + Translation2d m_frontLeftWheel; + Translation2d m_frontRightWheel; + Translation2d m_rearLeftWheel; + Translation2d m_rearRightWheel; + + Translation2d m_previousCoR; + + /** + * Construct inverse kinematics matrix from wheel locations. + * + * @param fl The location of the front-left wheel relative to the physical + * center of the robot. + * @param fr The location of the front-right wheel relative to the physical + * center of the robot. + * @param rl The location of the rear-left wheel relative to the physical + * center of the robot. + * @param rr The location of the rear-right wheel relative to the physical + * center of the robot. + */ + void SetInverseKinematics(Translation2d fl, Translation2d fr, + Translation2d rl, Translation2d rr); +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h new file mode 100644 index 0000000000..58bdc6cfc7 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -0,0 +1,94 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +namespace frc { + +/** + * Class for mecanum drive odometry. Odometry allows you to track the robot's + * position on the field over a course of a match using readings from your + * mecanum wheel encoders. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + */ +class MecanumDriveOdometry { + public: + /** + * Constructs a MecanumDriveOdometry object. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param initialPose The starting position of the robot on the field. + */ + explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics, + const Pose2d& initialPose = Pose2d()); + + /** + * Resets the robot's position on the field. + * + * @param pose The position on the field that your robot is at. + */ + void ResetPosition(const Pose2d& pose) { + m_pose = pose; + m_previousAngle = pose.Rotation(); + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param currentTime The current time. + * @param angle The angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * + * @return The new pose of the robot. + */ + const Pose2d& UpdateWithTime(units::second_t currentTime, + const Rotation2d& angle, + MecanumDriveWheelSpeeds wheelSpeeds); + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method automatically calculates + * the current time to calculate period (difference between two timestamps). + * The period is used to calculate the change in distance from a velocity. + * This also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param angle The angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * + * @return The new pose of the robot. + */ + const Pose2d& Update(const Rotation2d& angle, + MecanumDriveWheelSpeeds wheelSpeeds) { + const auto now = std::chrono::system_clock::now().time_since_epoch(); + units::second_t time{now}; + return UpdateWithTime(time, angle, wheelSpeeds); + } + + private: + MecanumDriveKinematics m_kinematics; + Pose2d m_pose; + + units::second_t m_previousTime = -1_s; + Rotation2d m_previousAngle; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h new file mode 100644 index 0000000000..159f7f0a64 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +namespace frc { +/** + * Represents the wheel speeds for a mecanum drive drivetrain. + */ +struct MecanumDriveWheelSpeeds { + /** + * Speed of the front-left wheel. + */ + units::meters_per_second_t frontLeft = 0_mps; + + /** + * Speed of the front-right wheel. + */ + units::meters_per_second_t frontRight = 0_mps; + + /** + * Speed of the rear-left wheel. + */ + units::meters_per_second_t rearLeft = 0_mps; + + /** + * Speed of the rear-right wheel. + */ + units::meters_per_second_t rearRight = 0_mps; + + /** + * Normalizes the wheel speeds using some max attainable speed. Sometimes, + * after inverse kinematics, the requested speed from a/several modules may be + * above the max attainable speed for the driving motor on that module. To fix + * this issue, one can "normalize" all the wheel speeds to make sure that all + * requested module speeds are below the absolute threshold, while maintaining + * the ratio of speeds between modules. + * + * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + */ + void Normalize(units::meters_per_second_t attainableMaxSpeed); +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h new file mode 100644 index 0000000000..fcd8087b65 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -0,0 +1,150 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include + +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/SwerveModuleState.h" + +namespace frc { +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual module states (speed and angle). + * + * The inverse kinematics (converting from a desired chassis velocity to + * individual module states) uses the relative locations of the modules with + * respect to the center of rotation. The center of rotation for inverse + * kinematics is also variable. This means that you can set your set your center + * of rotation in a corner of the robot to perform special evasion manuevers. + * + * Forward kinematics (converting an array of module states into the overall + * chassis motion) is performs the exact opposite of what inverse kinematics + * does. Since this is an overdetermined system (more equations than variables), + * we use a least-squares approximation. + * + * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] + * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then + * multiply by [moduleStates] to get our chassis speeds. + * + * Forward kinematics is also used for odometry -- determining the position of + * the robot on the field using encoders and a gyro. + */ +template +class SwerveDriveKinematics { + public: + /** + * Constructs a swerve drive kinematics object. This takes in a variable + * number of wheel locations as Translation2ds. The order in which you pass in + * the wheel locations is the same order that you will recieve the module + * states when performing inverse kinematics. It is also expected that you + * pass in the module states in the same order when calling the forward + * kinematics methods. + * + * @param wheels The locations of the wheels relative to the physical center + * of the robot. + */ + template + explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels) + : m_modules{wheel, wheels...} { + static_assert(sizeof...(wheels) >= 1, + "A swerve drive requires at least two modules"); + + for (size_t i = 0; i < NumModules; i++) { + // clang-format off + m_inverseKinematics.template block<2, 3>(i * 2, 0) << + 1, 0, (-m_modules[i].Y()).template to(), + 0, 1, (+m_modules[i].X()).template to(); + // clang-format on + } + + m_forwardKinematics = m_inverseKinematics.householderQr(); + } + + SwerveDriveKinematics(const SwerveDriveKinematics&) = default; + + /** + * Performs inverse kinematics to return the module states from a desired + * chassis velocity. This method is often used to convert joystick values into + * module speeds and angles. + * + * This function also supports variable centers of rotation. During normal + * operations, the center of rotation is usually the same as the physical + * center of the robot; therefore, the argument is defaulted to that use case. + * However, if you wish to change the center of rotation for evasive + * manuevers, vision alignment, or for any other use case, you can do so. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotation The center of rotation. For example, if you set the + * center of rotation at one corner of the robot and provide a chassis speed + * that only has a dtheta component, the robot will rotate around that corner. + * + * @return An array containing the module states. Use caution because these + * module states are not normalized. Sometimes, a user input may cause one of + * the module speeds to go above the attainable max velocity. Use the + * function to rectify this issue. In addition, you can + * leverage the power of C++17 to directly assign the module states to + * variables: + * + * @code{.cpp} + * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds); + * @endcode + */ + std::array ToSwerveModuleStates( + const ChassisSpeeds& chassisSpeeds, + const Translation2d& centerOfRotation = Translation2d()); + + /** + * Performs forward kinematics to return the resulting chassis state from the + * given module states. This method is often used for odometry -- determining + * the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param wheelStates The state of the modules (as a SwerveModuleState type) + * as measured from respective encoders and gyros. The order of the swerve + * module states should be same as passed into the constructor of this class. + * + * @return The resulting chassis speed. + */ + template + ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates); + + /** + * Normalizes the wheel speeds using some max attainable speed. Sometimes, + * after inverse kinematics, the requested speed from a/several modules may be + * above the max attainable speed for the driving motor on that module. To fix + * this issue, one can "normalize" all the wheel speeds to make sure that all + * requested module speeds are below the absolute threshold, while maintaining + * the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be + * mutated with the normalized speeds! + * @param attainableMaxSpeed The absolute max speed that a module can reach. + */ + static void NormalizeWheelSpeeds( + std::array* moduleStates, + units::meters_per_second_t attainableMaxSpeed); + + private: + Eigen::Matrix m_inverseKinematics; + Eigen::HouseholderQR> + m_forwardKinematics; + std::array m_modules; + + Translation2d m_previousCoR; +}; +} // namespace frc + +#include "SwerveDriveKinematics.inc" diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc new file mode 100644 index 0000000000..0fe3b232f9 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -0,0 +1,104 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +namespace frc { + +template +SwerveDriveKinematics(Translation2d, Wheels...) + ->SwerveDriveKinematics<1 + sizeof...(Wheels)>; + +template +std::array +SwerveDriveKinematics::ToSwerveModuleStates( + const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) { + // We have a new center of rotation. We need to compute the matrix again. + if (centerOfRotation.X() != m_previousCoR.X() || + centerOfRotation.Y() != m_previousCoR.Y()) { + for (size_t i = 0; i < NumModules; i++) { + // clang-format off + m_inverseKinematics.template block<2, 3>(i * 2, 0) << + 1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to(), + 0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to(); + // clang-format on + } + m_previousCoR = centerOfRotation; + } + + Eigen::Vector3d chassisSpeedsVector; + chassisSpeedsVector << chassisSpeeds.vx.to(), + chassisSpeeds.vy.to(), chassisSpeeds.omega.to(); + + Eigen::Matrix moduleStatesMatrix = + m_inverseKinematics * chassisSpeedsVector; + std::array moduleStates; + + for (size_t i = 0; i < NumModules; i++) { + units::meters_per_second_t x = + units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)}; + units::meters_per_second_t y = + units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)}; + + auto speed = units::math::hypot(x, y); + Rotation2d rotation{x.to(), y.to()}; + + moduleStates[i] = {speed, rotation}; + } + + return moduleStates; +} + +template +template +ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( + ModuleStates&&... wheelStates) { + static_assert(sizeof...(wheelStates) == NumModules, + "Number of modules is not consistent with number of wheel " + "locations provided in constructor."); + + std::array moduleStates{wheelStates...}; + Eigen::Matrix moduleStatesMatrix; + + for (size_t i = 0; i < NumModules; i++) { + SwerveModuleState module = moduleStates[i]; + moduleStatesMatrix.row(i * 2) + << module.speed.to() * module.angle.Cos(); + moduleStatesMatrix.row(i * 2 + 1) + << module.speed.to() * module.angle.Sin(); + } + + Eigen::Vector3d chassisSpeedsVector = + m_forwardKinematics.solve(moduleStatesMatrix); + + return {units::meters_per_second_t{chassisSpeedsVector(0)}, + units::meters_per_second_t{chassisSpeedsVector(1)}, + units::radians_per_second_t{chassisSpeedsVector(2)}}; +} + +template +void SwerveDriveKinematics::NormalizeWheelSpeeds( + std::array* moduleStates, + units::meters_per_second_t attainableMaxSpeed) { + auto& states = *moduleStates; + auto realMaxSpeed = std::max_element(states.begin(), states.end(), + [](const auto& a, const auto& b) { + return units::math::abs(a.speed) < + units::math::abs(b.speed); + }) + ->speed; + + if (realMaxSpeed > attainableMaxSpeed) { + for (auto& module : states) { + module.speed = module.speed / realMaxSpeed * attainableMaxSpeed; + } + } +} + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h new file mode 100644 index 0000000000..168c7c4cea --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -0,0 +1,107 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "SwerveDriveKinematics.h" +#include "SwerveModuleState.h" +#include "frc/geometry/Pose2d.h" + +namespace frc { + +/** + * Class for swerve drive odometry. Odometry allows you to track the robot's + * position on the field over a course of a match using readings from your + * swerve drive encoders and swerve azimuth encoders. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + */ +template +class SwerveDriveOdometry { + public: + /** + * Constructs a SwerveDriveOdometry object. + * + * @param kinematics The swerve drive kinematics for your drivetrain. + * @param initialPose The starting position of the robot on the field. + */ + SwerveDriveOdometry(SwerveDriveKinematics kinematics, + const Pose2d& initialPose = Pose2d()); + + /** + * Resets the robot's position on the field. + * + * @param pose The position on the field that your robot is at. + */ + void ResetPosition(const Pose2d& pose) { + m_pose = pose; + m_previousAngle = pose.Rotation(); + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param currentTime The current time. + * @param angle The angle of the robot. + * @param moduleStates The current state of all swerve modules. Please provide + * the states in the same order in which you instantiated + * your SwerveDriveKinematics. + * + * @return The new pose of the robot. + */ + template + const Pose2d& UpdateWithTime(units::second_t currentTime, + const Rotation2d& angle, + ModuleStates&&... moduleStates); + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method automatically calculates + * the current time to calculate period (difference between two timestamps). + * The period is used to calculate the change in distance from a velocity. + * This also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param angle The angle of the robot. + * @param moduleStates The current state of all swerve modules. Please provide + * the states in the same order in which you instantiated + * your SwerveDriveKinematics. + * + * @return The new pose of the robot. + */ + template + const Pose2d& Update(const Rotation2d& angle, + ModuleStates&&... moduleStates) { + const auto now = std::chrono::system_clock::now().time_since_epoch(); + units::second_t time{now}; + return UpdateWithTime(time, angle, moduleStates...); + } + + private: + SwerveDriveKinematics m_kinematics; + Pose2d m_pose; + + units::second_t m_previousTime = -1_s; + Rotation2d m_previousAngle; +}; + +} // namespace frc + +#include "SwerveDriveOdometry.inc" diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc new file mode 100644 index 0000000000..1ff75ac00b --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace frc { +template +SwerveDriveOdometry::SwerveDriveOdometry( + SwerveDriveKinematics kinematics, const Pose2d& initialPose) + : m_kinematics(kinematics), m_pose(initialPose) { + m_previousAngle = m_pose.Rotation(); +} + +template +template +const Pose2d& frc::SwerveDriveOdometry::UpdateWithTime( + units::second_t currentTime, const Rotation2d& angle, + ModuleStates&&... moduleStates) { + units::second_t deltaTime = + (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; + m_previousTime = currentTime; + + auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...); + static_cast(dtheta); + + auto newPose = m_pose.Exp( + {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()}); + + m_previousAngle = angle; + m_pose = {newPose.Translation(), angle}; + + return m_pose; +} +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h new file mode 100644 index 0000000000..fbfe0d1da9 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/geometry/Rotation2d.h" + +namespace frc { +/** + * Represents the state of one swerve module. + */ +struct SwerveModuleState { + /** + * Speed of the wheel of the module. + */ + units::meters_per_second_t speed = 0_mps; + + /** + * Angle of the module. + */ + Rotation2d angle; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp new file mode 100644 index 0000000000..3fb63fbd87 --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/ChassisSpeeds.h" +#include "gtest/gtest.h" + +static constexpr double kEpsilon = 1E-9; + +TEST(ChassisSpeeds, FieldRelativeConstruction) { + const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds( + 1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg)); + + EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), kEpsilon); + EXPECT_NEAR(1.0, chassisSpeeds.vy.to(), kEpsilon); + EXPECT_NEAR(0.5, chassisSpeeds.omega.to(), kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp new file mode 100644 index 0000000000..ad0d3c7592 --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -0,0 +1,77 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const ChassisSpeeds chassisSpeeds; + const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); + + EXPECT_NEAR(wheelSpeeds.left.to(), 0, kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.to(), 0, kEpsilon); +} + +TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const DifferentialDriveWheelSpeeds wheelSpeeds; + const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 0, kEpsilon); +} + +TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s}; + const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); + + EXPECT_NEAR(wheelSpeeds.left.to(), 3, kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.to(), 3, kEpsilon); +} + +TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps}; + const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 3, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 0, kEpsilon); +} + +TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps, + units::radians_per_second_t{wpi::math::pi}}; + const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); + + EXPECT_NEAR(wheelSpeeds.left.to(), -0.381 * wpi::math::pi, kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.to(), +0.381 * wpi::math::pi, kEpsilon); +} + +TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const DifferentialDriveWheelSpeeds wheelSpeeds{ + units::meters_per_second_t(+0.381 * wpi::math::pi), + units::meters_per_second_t(-0.381 * wpi::math::pi)}; + const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), -wpi::math::pi, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp new file mode 100644 index 0000000000..2647252a2d --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveOdometry.h" +#include "gtest/gtest.h" + +static constexpr double kEpsilon = 1E-9; + +using namespace frc; + +TEST(DifferentialDriveOdometry, OneIteration) { + DifferentialDriveKinematics kinematics{0.381_m * 2}; + DifferentialDriveOdometry odometry{kinematics}; + + odometry.ResetPosition(Pose2d()); + DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps}; + odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds()); + const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds); + + EXPECT_NEAR(pose.Translation().X().to(), 0.02, kEpsilon); + EXPECT_NEAR(pose.Translation().Y().to(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, kEpsilon); +} + +TEST(DifferentialDriveOdometry, QuarterCircle) { + DifferentialDriveKinematics kinematics{0.381_m * 2}; + DifferentialDriveOdometry odometry{kinematics}; + + odometry.ResetPosition(Pose2d()); + DifferentialDriveWheelSpeeds wheelSpeeds{ + 0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)}; + odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds()); + const auto& pose = + odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds); + + EXPECT_NEAR(pose.Translation().X().to(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Translation().Y().to(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp new file mode 100644 index 0000000000..75f395d72a --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -0,0 +1,230 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "frc/geometry/Translation2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "gtest/gtest.h" + +using namespace frc; + +class MecanumDriveKinematicsTest : public ::testing::Test { + protected: + Translation2d m_fl{12_m, 12_m}; + Translation2d m_fr{12_m, -12_m}; + Translation2d m_bl{-12_m, 12_m}; + Translation2d m_br{-12_m, -12_m}; + + MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br}; +}; + +TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) { + ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s}; + auto moduleStates = kinematics.ToWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 + */ + + EXPECT_NEAR(3.536, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(3.536, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(3.536, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(3.536, moduleStates.rearRight.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) { + MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps, + 3.536_mps}; + auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be + [[5][0][0]] + */ + + EXPECT_NEAR(5.0, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.omega.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) { + ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s}; + auto moduleStates = kinematics.ToWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427 + */ + + EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(2.828427, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(2.828427, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(-2.828427, moduleStates.rearRight.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) { + MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps, + -2.828427_mps}; + auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be + [[5][0][0]] + */ + + EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(4.0, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.omega.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) { + ChassisSpeeds speeds{0_mps, 0_mps, + units::radians_per_second_t(2 * wpi::math::pi)}; + auto moduleStates = kinematics.ToWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 + */ + + EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(106.62919, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(106.62919, moduleStates.rearRight.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) { + MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps, + -106.62919_mps, 106.62919_mps}; + auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should + be [[0][0][2pi]] + */ + + EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) { + ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s}; + auto moduleStates = kinematics.ToWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 + */ + + EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(20.506097, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-13.435, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(16.26, moduleStates.rearRight.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) { + MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps, + -13.435_mps, 16.26_mps}; + + auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be + [[2][3][1]] + */ + + EXPECT_NEAR(2.0, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(3.0, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(1.0, chassisSpeeds.omega.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) { + ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s}; + auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125 + */ + + EXPECT_NEAR(0, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(16.971, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-16.971, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(33.941, moduleStates.rearRight.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) { + MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps, + 33.941_mps}; + auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from the + wheel velocities should be [[12][-12][1]] + */ + + EXPECT_NEAR(12.0, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(-12, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(1.0, chassisSpeeds.omega.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, + OffCenterTranslationRotationInverseKinematics) { + ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s}; + auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446 + */ + EXPECT_NEAR(2.12, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(21.92, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-12.02, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(36.06, moduleStates.rearRight.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, + OffCenterTranslationRotationForwardKinematics) { + MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps, + 36.06_mps}; + auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from the + wheel velocities should be [[17][-10][1]] + */ + + EXPECT_NEAR(17.0, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(-10, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(1.0, chassisSpeeds.omega.to(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, NormalizeTest) { + MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}; + wheelSpeeds.Normalize(5.5_mps); + + double kFactor = 5.5 / 7.0; + + EXPECT_NEAR(wheelSpeeds.frontLeft.to(), 5.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.frontRight.to(), 6.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.rearLeft.to(), 4.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.rearRight.to(), 7.0 * kFactor, 1E-9); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp new file mode 100644 index 0000000000..0a6894dcae --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/MecanumDriveOdometry.h" +#include "gtest/gtest.h" + +using namespace frc; + +class MecanumDriveOdometryTest : public ::testing::Test { + protected: + Translation2d m_fl{12_m, 12_m}; + Translation2d m_fr{12_m, -12_m}; + Translation2d m_bl{-12_m, 12_m}; + Translation2d m_br{-12_m, -12_m}; + + MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br}; + MecanumDriveOdometry odometry{kinematics}; +}; + +TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) { + odometry.ResetPosition(Pose2d()); + MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps, + 3.536_mps}; + + odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds); + auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds); + + EXPECT_NEAR(secondPose.Translation().X().to(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Translation().Y().to(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Rotation().Radians().to(), 0.0, 0.01); +} + +TEST_F(MecanumDriveOdometryTest, TwoIterations) { + odometry.ResetPosition(Pose2d()); + MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps}; + + odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); + auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds); + + EXPECT_NEAR(pose.Translation().X().to(), 0.5, 0.01); + EXPECT_NEAR(pose.Translation().Y().to(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); +} + +TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) { + odometry.ResetPosition(Pose2d()); + MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps, + 39.986_mps}; + odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); + auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds); + + EXPECT_NEAR(pose.Translation().X().to(), 12, 0.01); + EXPECT_NEAR(pose.Translation().Y().to(), 12, 0.01); + EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, 0.01); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp new file mode 100644 index 0000000000..8fd67ea4ac --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -0,0 +1,183 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "frc/geometry/Translation2d.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 0.1; + +class SwerveDriveKinematicsTest : public ::testing::Test { + protected: + Translation2d m_fl{12_m, 12_m}; + Translation2d m_fr{12_m, -12_m}; + Translation2d m_bl{-12_m, 12_m}; + Translation2d m_br{-12_m, -12_m}; + + SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br}; +}; + +TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) { + ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s}; + + auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); + + EXPECT_NEAR(fl.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(fr.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(bl.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(br.speed.to(), 5.0, kEpsilon); + + EXPECT_NEAR(fl.angle.Radians().to(), 0.0, kEpsilon); + EXPECT_NEAR(fr.angle.Radians().to(), 0.0, kEpsilon); + EXPECT_NEAR(bl.angle.Radians().to(), 0.0, kEpsilon); + EXPECT_NEAR(br.angle.Radians().to(), 0.0, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) { + SwerveModuleState state{5.0_mps, Rotation2d()}; + + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 5.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 0.0, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) { + ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s}; + auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); + + EXPECT_NEAR(fl.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(fr.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(bl.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(br.speed.to(), 5.0, kEpsilon); + + EXPECT_NEAR(fl.angle.Degrees().to(), 90.0, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().to(), 90.0, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().to(), 90.0, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().to(), 90.0, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) { + SwerveModuleState state{5_mps, Rotation2d(90_deg)}; + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), 5.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 0.0, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) { + ChassisSpeeds speeds{0_mps, 0_mps, + units::radians_per_second_t(2 * wpi::math::pi)}; + auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); + + EXPECT_NEAR(fl.speed.to(), 106.63, kEpsilon); + EXPECT_NEAR(fr.speed.to(), 106.63, kEpsilon); + EXPECT_NEAR(bl.speed.to(), 106.63, kEpsilon); + EXPECT_NEAR(br.speed.to(), 106.63, kEpsilon); + + EXPECT_NEAR(fl.angle.Degrees().to(), 135.0, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().to(), 45.0, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().to(), -135.0, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().to(), -45.0, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) { + SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)}; + SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)}; + SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)}; + SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)}; + + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::math::pi, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) { + ChassisSpeeds speeds{0_mps, 0_mps, + units::radians_per_second_t(2 * wpi::math::pi)}; + auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl); + + EXPECT_NEAR(fl.speed.to(), 0.0, kEpsilon); + EXPECT_NEAR(fr.speed.to(), 150.796, kEpsilon); + EXPECT_NEAR(bl.speed.to(), 150.796, kEpsilon); + EXPECT_NEAR(br.speed.to(), 213.258, kEpsilon); + + EXPECT_NEAR(fl.angle.Degrees().to(), 0.0, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().to(), 0.0, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().to(), -90.0, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().to(), -45.0, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) { + SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)}; + SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)}; + SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)}; + SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)}; + + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 75.398, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), -75.398, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::math::pi, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, + OffCenterCORRotationAndTranslationInverseKinematics) { + ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s}; + auto [fl, fr, bl, br] = + m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m)); + + EXPECT_NEAR(fl.speed.to(), 23.43, kEpsilon); + EXPECT_NEAR(fr.speed.to(), 23.43, kEpsilon); + EXPECT_NEAR(bl.speed.to(), 54.08, kEpsilon); + EXPECT_NEAR(br.speed.to(), 54.08, kEpsilon); + + EXPECT_NEAR(fl.angle.Degrees().to(), -140.19, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().to(), -39.81, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().to(), -109.44, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().to(), -70.56, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, + OffCenterCORRotationAndTranslationForwardKinematics) { + SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)}; + SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)}; + SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)}; + SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)}; + + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br); + + EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.to(), -33.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 1.5, kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, NormalizeTest) { + SwerveModuleState state1{5.0_mps, Rotation2d()}; + SwerveModuleState state2{6.0_mps, Rotation2d()}; + SwerveModuleState state3{4.0_mps, Rotation2d()}; + SwerveModuleState state4{7.0_mps, Rotation2d()}; + + std::array arr{state1, state2, state3, state4}; + SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps); + + double kFactor = 5.5 / 7.0; + + EXPECT_NEAR(arr[0].speed.to(), 5.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[1].speed.to(), 6.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[2].speed.to(), 4.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[3].speed.to(), 7.0 * kFactor, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp new file mode 100644 index 0000000000..ab81017ed7 --- /dev/null +++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/SwerveDriveOdometry.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 0.01; + +class SwerveDriveOdometryTest : public ::testing::Test { + protected: + Translation2d m_fl{12_m, 12_m}; + Translation2d m_fr{12_m, -12_m}; + Translation2d m_bl{-12_m, 12_m}; + Translation2d m_br{-12_m, -12_m}; + + SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br}; + SwerveDriveOdometry<4> m_odometry{m_kinematics}; +}; + +TEST_F(SwerveDriveOdometryTest, TwoIterations) { + SwerveModuleState state{5_mps, Rotation2d()}; + + m_odometry.ResetPosition(Pose2d()); + m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(), + SwerveModuleState(), SwerveModuleState(), + SwerveModuleState()); + auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state, + state, state); + + EXPECT_NEAR(0.5, pose.Translation().X().to(), kEpsilon); + EXPECT_NEAR(0.0, pose.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().Degrees().to(), kEpsilon); +} + +TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { + SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)}; + SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)}; + SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)}; + SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)}; + + SwerveModuleState zero{0_mps, Rotation2d()}; + + m_odometry.ResetPosition(Pose2d()); + m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero); + auto pose = + m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br); + + EXPECT_NEAR(12.0, pose.Translation().X().to(), kEpsilon); + EXPECT_NEAR(12.0, pose.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(90.0, pose.Rotation().Degrees().to(), kEpsilon); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..8dcc1ea00c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Drivetrain.h" + +frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const { + return {units::meters_per_second_t(m_leftEncoder.GetRate()), + units::meters_per_second_t(m_rightEncoder.GetRate())}; +} + +void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { + const auto leftOutput = m_leftPIDController.Calculate( + m_leftEncoder.GetRate(), speeds.left.to()); + const auto rightOutput = m_rightPIDController.Calculate( + m_rightEncoder.GetRate(), speeds.right.to()); + + m_leftGroup.Set(leftOutput); + m_rightGroup.Set(rightOutput); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot) { + SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); +} + +void Drivetrain::UpdateOdometry() { + m_odometry.Update(GetAngle(), GetSpeeds()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp new file mode 100644 index 0000000000..b1e858ee47 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + TeleopPeriodic(); + m_drive.UpdateOdometry(); + } + + void TeleopPeriodic() override { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = + -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) * + Drivetrain::kMaxAngularSpeed; + + m_drive.Drive(xSpeed, rot); + } + + private: + frc::XboxController m_controller{0}; + Drivetrain m_drive; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h new file mode 100644 index 0000000000..bf2ad98def --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Represents a differential drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { + m_gyro.Reset(); + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + } + + /** + * Get the robot angle as a Rotation2d. + */ + frc::Rotation2d GetAngle() const { + // Negating the angle because WPILib Gyros are CW positive. + return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle())); + } + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + frc::DifferentialDriveWheelSpeeds GetSpeeds() const; + void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); + void Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot); + void UpdateOdometry(); + + private: + static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr double kWheelRadius = 0.0508; // meters + static constexpr int kEncoderResolution = 4096; + + frc::Spark m_leftMaster{1}; + frc::Spark m_leftFollower{2}; + frc::Spark m_rightMaster{3}; + frc::Spark m_rightFollower{4}; + + frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower}; + frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower}; + + frc::Encoder m_leftEncoder{0, 1}; + frc::Encoder m_rightEncoder{0, 1}; + + frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + frc::DifferentialDriveOdometry m_odometry{m_kinematics}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..5e4fc8c608 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Drivetrain.h" + +frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { + return {units::meters_per_second_t(m_frontLeftEncoder.GetRate()), + units::meters_per_second_t(m_frontRightEncoder.GetRate()), + units::meters_per_second_t(m_backLeftEncoder.GetRate()), + units::meters_per_second_t(m_backRightEncoder.GetRate())}; +} + +void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { + const auto frontLeftOutput = m_frontLeftPIDController.Calculate( + m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to()); + const auto frontRightOutput = m_frontRightPIDController.Calculate( + m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to()); + const auto backLeftOutput = m_backLeftPIDController.Calculate( + m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to()); + const auto backRightOutput = m_backRightPIDController.Calculate( + m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to()); + + m_frontLeftMotor.Set(frontLeftOutput); + m_frontRightMotor.Set(frontRightOutput); + m_backLeftMotor.Set(backLeftOutput); + m_backRightMotor.Set(backRightOutput); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, bool fieldRelative) { + auto wheelSpeeds = m_kinematics.ToWheelSpeeds( + fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, GetAngle()) + : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); + wheelSpeeds.Normalize(kMaxSpeed); + SetSpeeds(wheelSpeeds); +} + +void Drivetrain::UpdateOdometry() { + m_odometry.Update(GetAngle(), GetCurrentState()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp new file mode 100644 index 0000000000..5ff14d1970 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + DriveWithJoystick(false); + m_mecanum.UpdateOdometry(); + } + + void TeleopPeriodic() override { DriveWithJoystick(true); } + + private: + frc::XboxController m_controller{0}; + Drivetrain m_mecanum; + + void DriveWithJoystick(bool fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = + -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + const auto ySpeed = + -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) * + Drivetrain::kMaxAngularSpeed; + + m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h new file mode 100644 index 0000000000..3d44730f17 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Represents a mecanum drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { m_gyro.Reset(); } + + /** + * Get the robot angle as a Rotation2d + */ + frc::Rotation2d GetAngle() const { + // Negating the angle because WPILib Gyros are CW positive. + return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle())); + } + + frc::MecanumDriveWheelSpeeds GetCurrentState() const; + void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); + void Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, units::radians_per_second_t rot, + bool fieldRelative); + void UpdateOdometry(); + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + private: + frc::Spark m_frontLeftMotor{1}; + frc::Spark m_frontRightMotor{2}; + frc::Spark m_backLeftMotor{3}; + frc::Spark m_backRightMotor{4}; + + frc::Encoder m_frontLeftEncoder{0, 1}; + frc::Encoder m_frontRightEncoder{0, 1}; + frc::Encoder m_backLeftEncoder{0, 1}; + frc::Encoder m_backRightEncoder{0, 1}; + + frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m}; + frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m}; + frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m}; + frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m}; + + frc2::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_frontRightPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_backLeftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_backRightPIDController{1.0, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::MecanumDriveKinematics m_kinematics{ + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, + m_backRightLocation}; + + frc::MecanumDriveOdometry m_odometry{m_kinematics}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..416d3032a3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Drivetrain.h" + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, bool fieldRelative) { + auto states = m_kinematics.ToSwerveModuleStates( + fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, GetAngle()) + : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); + + m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed); + + auto [fl, fr, bl, br] = states; + + m_frontLeft.SetDesiredState(fl); + m_frontRight.SetDesiredState(fr); + m_backLeft.SetDesiredState(bl); + m_backRight.SetDesiredState(br); +} + +void Drivetrain::UpdateOdometry() { + m_odometry.Update(GetAngle(), m_frontLeft.GetState(), m_frontRight.GetState(), + m_backLeft.GetState(), m_backRight.GetState()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp new file mode 100644 index 0000000000..7376e9e95a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + DriveWithJoystick(false); + m_swerve.UpdateOdometry(); + } + + void TeleopPeriodic() override { DriveWithJoystick(true); } + + private: + frc::XboxController m_controller{0}; + Drivetrain m_swerve; + + void DriveWithJoystick(bool fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = + -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + const auto ySpeed = + -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) * + Drivetrain::kMaxAngularSpeed; + + m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp new file mode 100644 index 0000000000..3002688aa3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "SwerveModule.h" + +#include +#include + +SwerveModule::SwerveModule(const int driveMotorChannel, + const int turningMotorChannel) + : m_driveMotor(driveMotorChannel), m_turningMotor(turningMotorChannel) { + // Set the distance per pulse for the drive encoder. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_driveEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + + // Set the distance (in this case, angle) per pulse for the turning encoder. + // This is the the angle through an entire rotation (2 * wpi::math::pi) + // divided by the encoder resolution. + m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution); + + // Limit the PID Controller's input range between -pi and pi and set the input + // to be continuous. + m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi); +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return {units::meters_per_second_t{m_driveEncoder.GetRate()}, + frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))}; +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) { + // Calculate the drive output from the drive PID controller. + const auto driveOutput = m_drivePIDController.Calculate( + m_driveEncoder.GetRate(), state.speed.to()); + + // Calculate the turning motor output from the turning PID controller. + const auto turnOutput = m_turningPIDController.Calculate( + m_turningEncoder.Get(), + // We have to convert to the meters type here because that's what + // ProfiledPIDController wants. + units::meter_t(state.angle.Radians().to())); + + // Set the motor outputs. + m_driveMotor.Set(driveOutput); + m_turningMotor.Set(turnOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h new file mode 100644 index 0000000000..745581ac8b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SwerveModule.h" + +/** + * Represents a swerve drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { m_gyro.Reset(); } + + /** + * Get the robot angle as a Rotation2d. + */ + frc::Rotation2d GetAngle() const { + // Negating the angle because WPILib Gyros are CW positive. + return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle())); + } + + void Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, units::radians_per_second_t rot, + bool fieldRelative); + void UpdateOdometry(); + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + private: + frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m}; + frc::Translation2d m_frontRightLocation{+0.381_m, -0.381_m}; + frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m}; + frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m}; + + SwerveModule m_frontLeft{1, 2}; + SwerveModule m_frontRight{2, 3}; + SwerveModule m_backLeft{5, 6}; + SwerveModule m_backRight{7, 8}; + + frc::AnalogGyro m_gyro{0}; + + frc::SwerveDriveKinematics<4> m_kinematics{ + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, + m_backRightLocation}; + + frc::SwerveDriveOdometry<4> m_odometry{m_kinematics}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h new file mode 100644 index 0000000000..0eaa69ee92 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +class SwerveModule { + public: + SwerveModule(int driveMotorChannel, int turningMotorChannel); + frc::SwerveModuleState GetState() const; + void SetDesiredState(const frc::SwerveModuleState& state); + + private: + static constexpr double kWheelRadius = 0.0508; + static constexpr int kEncoderResolution = 4096; + + // We have to use meters here instead of radians because of the fact that + // ProfiledPIDController's constraints only take in meters per second and + // meters per second squared. + + static constexpr units::meters_per_second_t kModuleMaxAngularVelocity = + units::meters_per_second_t(wpi::math::pi); // radians per second + static constexpr units::meters_per_second_squared_t + kModuleMaxAngularAcceleration = units::meters_per_second_squared_t( + wpi::math::pi * 2.0); // radians per second squared + + frc::Spark m_driveMotor; + frc::Spark m_turningMotor; + + frc::Encoder m_driveEncoder{0, 1}; + frc::Encoder m_turningEncoder{0, 1}; + + frc2::PIDController m_drivePIDController{1.0, 0, 0}; + frc::ProfiledPIDController m_turningPIDController{ + 1.0, + 0.0, + 0.0, + {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 4e1bb834ce..df3e3cc313 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -328,5 +328,32 @@ ], "foldername": "GyroDriveCommands", "mainclass": "Main" + }, + { + "name": "SwerveBot", + "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.", + "tags": [ + "SwerveBot" + ], + "foldername": "SwerveBot", + "gradlebase": "cpp" + }, + { + "name": "MecanumBot", + "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.", + "tags": [ + "MecanumBot" + ], + "foldername": "MecanumBot", + "gradlebase": "cpp" + }, + { + "name": "DifferentialDriveBot", + "description": "An example program for a differential drive that uses differential drive kinematics and odometry.", + "tags": [ + "DifferentialDriveBot" + ], + "foldername": "DifferentialDriveBot", + "gradlebase": "cpp" } ] diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java new file mode 100644 index 0000000000..e16f0a0d64 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java @@ -0,0 +1,85 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + + +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +/** + * Represents the speed of a robot chassis. Although this struct contains + * similar members compared to a Twist2d, they do NOT represent the same thing. + * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference, + * this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of + * reference. + * + *

A strictly non-holonomic drivetrain, such as a differential drive, should + * never have a dy component because it can never move sideways. Holonomic + * drivetrains such as swerve and mecanum will often have all three components. + */ +@SuppressWarnings("MemberName") +public class ChassisSpeeds { + /** + * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) + */ + public double vxMetersPerSecond; + + /** + * Represents sideways velocity w.r.t the robot frame of reference. (Left is +) + */ + public double vyMetersPerSecond; + + /** + * Represents the angular velocity of the robot frame. (CCW is +) + */ + public double omegaRadiansPerSecond; + + /** + * Constructs a ChassisSpeeds with zeros for dx, dy, and theta. + */ + public ChassisSpeeds() { + } + + /** + * Constructs a ChassisSpeeds object. + * + * @param vxMetersPerSecond Forward velocity. + * @param vyMetersPerSecond Sideways velocity. + * @param omegaRadiansPerSecond Angular velocity. + */ + public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond, + double omegaRadiansPerSecond) { + this.vxMetersPerSecond = vxMetersPerSecond; + this.vyMetersPerSecond = vyMetersPerSecond; + this.omegaRadiansPerSecond = omegaRadiansPerSecond; + } + + /** + * Converts a user provided field-relative set of speeds into a robot-relative + * ChassisSpeeds object. + * + * @param vxMetersPerSecond The component of speed in the x direction relative to the field. + * Positive x is away from your alliance wall. + * @param vyMetersPerSecond The component of speed in the y direction relative to the field. + * Positive y is to your left when standing behind the alliance wall. + * @param omegaRadiansPerSecond The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's + * angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should + * be CCW positive. + * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + */ + public static ChassisSpeeds fromFieldRelativeSpeeds( + double vxMetersPerSecond, double vyMetersPerSecond, + double omegaRadiansPerSecond, Rotation2d robotAngle) { + return new ChassisSpeeds( + vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(), + -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(), + omegaRadiansPerSecond + ); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java new file mode 100644 index 0000000000..249cea7141 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +/** + * Helper class that converts a chassis velocity (dx and dtheta components) to + * left and right wheel velocities for a differential drive. + * + *

Inverse kinematics converts a desired chassis speed into left and right + * velocity components whereas forward kinematics converts left and right + * component velocities into a linear and angular chassis speed. + */ +public class DifferentialDriveKinematics { + private final double m_trackWidthMeters; + + /** + * Constructs a differential drive kinematics object. + * + * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is + * the distance between the left wheels and right wheels. + * However, the empirical value may be larger than the physical + * measured value due to scrubbing effects. + */ + public DifferentialDriveKinematics(double trackWidthMeters) { + m_trackWidthMeters = trackWidthMeters; + } + + /** + * Returns a chassis speed from left and right component velocities using + * forward kinematics. + * + * @param wheelSpeeds The left and right velocities. + * @return The chassis speed. + */ + public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) { + return new ChassisSpeeds( + (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0, + (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) + / m_trackWidthMeters + ); + } + + /** + * Returns left and right component velocities from a chassis speed using + * inverse kinematics. + * + * @param chassisSpeeds The linear and angular (dx and dtheta) components that + * represent the chassis' speed. + * @return The left and right velocities. + */ + public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { + return new DifferentialDriveWheelSpeeds( + chassisSpeeds.vxMetersPerSecond - m_trackWidthMeters / 2 + * chassisSpeeds.omegaRadiansPerSecond, + chassisSpeeds.vxMetersPerSecond + m_trackWidthMeters / 2 + * chassisSpeeds.omegaRadiansPerSecond + ); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java new file mode 100644 index 0000000000..53eeff2c01 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java @@ -0,0 +1,114 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Twist2d; + +/** + * Class for differential drive odometry. Odometry allows you to track the + * robot's position on the field over the course of a match using readings from + * 2 encoders and a gyroscope. + * + *

Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + * + *

Note: It is important to reset both your encoders to zero before you start + * using this class. Only reset your encoders ONCE. You should not reset your + * encoders even if you want to reset your robot's pose. + */ +public class DifferentialDriveOdometry { + private final DifferentialDriveKinematics m_kinematics; + private Pose2d m_poseMeters; + private double m_prevTimeSeconds = -1; + + private Rotation2d m_previousAngle; + + /** + * Constructs a DifferentialDriveOdometry object. + * + * @param kinematics The differential drive kinematics for your drivetrain. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, + Pose2d initialPoseMeters) { + m_kinematics = kinematics; + m_poseMeters = initialPoseMeters; + m_previousAngle = initialPoseMeters.getRotation(); + } + + /** + * Constructs a DifferentialDriveOdometry object with the default pose at the origin. + * + * @param kinematics The differential drive kinematics for your drivetrain. + */ + public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics) { + this(kinematics, new Pose2d()); + } + + /** + * Resets the robot's position on the field. Do NOT zero your encoders if you + * call this function at any other time except initialization. + * + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Pose2d poseMeters) { + m_poseMeters = poseMeters; + m_previousAngle = poseMeters.getRotation(); + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param currentTimeSeconds The current time in seconds. + * @param angle The current robot angle. + * @param wheelSpeeds The current wheel speeds. + * @return The new pose of the robot. + */ + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle, + DifferentialDriveWheelSpeeds wheelSpeeds) { + double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; + m_prevTimeSeconds = currentTimeSeconds; + + var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds); + var newPose = m_poseMeters.exp( + new Twist2d(chassisState.vxMetersPerSecond * period, + chassisState.vyMetersPerSecond * period, + angle.minus(m_previousAngle).getRadians())); + + m_previousAngle = angle; + + m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + return m_poseMeters; + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method automatically calculates the + * current time to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param angle The angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d angle, + DifferentialDriveWheelSpeeds wheelSpeeds) { + return updateWithTime(System.currentTimeMillis() / 1000.0, + angle, wheelSpeeds); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java new file mode 100644 index 0000000000..32c5ade93c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +/** + * Represents the wheel speeds for a differential drive drivetrain. + */ +@SuppressWarnings("MemberName") +public class DifferentialDriveWheelSpeeds { + /** + * Speed of the left side of the robot. + */ + public double leftMetersPerSecond; + + /** + * Speed of the right side of the robot. + */ + public double rightMetersPerSecond; + + /** + * Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. + */ + public DifferentialDriveWheelSpeeds() { + } + + /** + * Constructs a DifferentialDriveWheelSpeeds. + * + * @param leftMetersPerSecond The left speed. + * @param rightMetersPerSecond The right speed. + */ + public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) { + this.leftMetersPerSecond = leftMetersPerSecond; + this.rightMetersPerSecond = rightMetersPerSecond; + } + + /** + * Normalizes the wheel speeds using some max attainable speed. Sometimes, + * after inverse kinematics, the requested speed from a/several modules may be + * above the max attainable speed for the driving motor on that module. To fix + * this issue, one can "normalize" all the wheel speeds to make sure that all + * requested module speeds are below the absolute threshold, while maintaining + * the ratio of speeds between modules. + * + * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. + */ + public void normalize(double attainableMaxSpeedMetersPerSecond) { + double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond)); + + if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { + leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java new file mode 100644 index 0000000000..4b0370e014 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java @@ -0,0 +1,169 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.geometry.Translation2d; + +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual wheel speeds. + * + *

The inverse kinematics (converting from a desired chassis velocity to + * individual wheel speeds) uses the relative locations of the wheels with + * respect to the center of rotation. The center of rotation for inverse + * kinematics is also variable. This means that you can set your set your center + * of rotation in a corner of the robot to perform special evasion manuevers. + * + *

Forward kinematics (converting an array of wheel speeds into the overall + * chassis motion) is performs the exact opposite of what inverse kinematics + * does. Since this is an overdetermined system (more equations than variables), + * we use a least-squares approximation. + * + *

The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] + * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then + * multiply by [wheelSpeeds] to get our chassis speeds. + * + *

Forward kinematics is also used for odometry -- determining the position of + * the robot on the field using encoders and a gyro. + */ +public class MecanumDriveKinematics { + private SimpleMatrix m_inverseKinematics; + private final SimpleMatrix m_forwardKinematics; + + private final Translation2d m_frontLeftWheelMeters; + private final Translation2d m_frontRightWheelMeters; + private final Translation2d m_rearLeftWheelMeters; + private final Translation2d m_rearRightWheelMeters; + + private Translation2d m_prevCoR = new Translation2d(); + + /** + * Constructs a mecanum drive kinematics object. + * + * @param frontLeftWheelMeters The location of the front-left wheel relative to the + * physical center of the robot. + * @param frontRightWheelMeters The location of the front-right wheel relative to + * the physical center of the robot. + * @param rearLeftWheelMeters The location of the rear-left wheel relative to the + * physical center of the robot. + * @param rearRightWheelMeters The location of the rear-right wheel relative to the + * physical center of the robot. + */ + public MecanumDriveKinematics(Translation2d frontLeftWheelMeters, + Translation2d frontRightWheelMeters, + Translation2d rearLeftWheelMeters, + Translation2d rearRightWheelMeters) { + m_frontLeftWheelMeters = frontLeftWheelMeters; + m_frontRightWheelMeters = frontRightWheelMeters; + m_rearLeftWheelMeters = rearLeftWheelMeters; + m_rearRightWheelMeters = rearRightWheelMeters; + + m_inverseKinematics = new SimpleMatrix(4, 3); + + setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters, + rearLeftWheelMeters, rearRightWheelMeters); + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + } + + /** + * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This + * method is often used to convert joystick values into wheel speeds. + * + *

This function also supports variable centers of rotation. During normal + * operations, the center of rotation is usually the same as the physical + * center of the robot; therefore, the argument is defaulted to that use case. + * However, if you wish to change the center of rotation for evasive + * manuevers, vision alignment, or for any other use case, you can do so. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotationMeters The center of rotation. For example, if you set the + * center of rotation at one corner of the robot and provide + * a chassis speed that only has a dtheta component, the robot + * will rotate around that corner. + * @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user + * input may cause one of the wheel speeds to go above the attainable max velocity. Use + * the {@link MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue. + */ + public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds, + Translation2d centerOfRotationMeters) { + // We have a new center of rotation. We need to compute the matrix again. + if (centerOfRotationMeters.getX() != m_prevCoR.getX() + || centerOfRotationMeters.getY() != m_prevCoR.getY()) { + var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters); + var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters); + var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters); + var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters); + + setInverseKinematics(fl, fr, rl, rr); + m_prevCoR = centerOfRotationMeters; + } + + var chassisSpeedsVector = new SimpleMatrix(3, 1); + chassisSpeedsVector.setColumn(0, 0, + chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); + + var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector); + return new MecanumDriveWheelSpeeds( + wheelsMatrix.get(0, 0), + wheelsMatrix.get(1, 0), + wheelsMatrix.get(2, 0), + wheelsMatrix.get(3, 0) + ); + } + + /** + * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more + * information. + * + * @param chassisSpeeds The desired chassis speed. + * @return The wheel speeds. + */ + public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { + return toWheelSpeeds(chassisSpeeds, new Translation2d()); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the given wheel speeds. + * This method is often used for odometry -- determining the robot's position on the field using + * data from the real-world speed of each wheel on the robot. + * + * @param wheelSpeeds The current mecanum drive wheel speeds. + * @return The resulting chassis speed. + */ + public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { + var wheelSpeedsMatrix = new SimpleMatrix(4, 1); + wheelSpeedsMatrix.setColumn(0, 0, + wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond, + wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond + ); + var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix); + + return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0), + chassisSpeedsVector.get(2, 0)); + } + + /** + * Construct inverse kinematics matrix from wheel locations. + * + * @param fl The location of the front-left wheel relative to the physical center of the robot. + * @param fr The location of the front-right wheel relative to the physical center of the robot. + * @param rl The location of the rear-left wheel relative to the physical center of the robot. + * @param rr The location of the rear-right wheel relative to the physical center of the robot. + */ + private void setInverseKinematics(Translation2d fl, Translation2d fr, + Translation2d rl, Translation2d rr) { + m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY())); + m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY()); + m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY()); + m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY())); + m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2)); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java new file mode 100644 index 0000000000..f43e59094f --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java @@ -0,0 +1,107 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Twist2d; + +/** + * Class for mecnaum drive odometry. Odometry allows you to track the robot's + * position on the field over a course of a match using readings from your + * mecanum wheel encoders. + * + *

Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + */ +public class MecanumDriveOdometry { + private final MecanumDriveKinematics m_kinematics; + private Pose2d m_poseMeters; + private double m_prevTimeSeconds = -1; + + private Rotation2d m_previousAngle; + + /** + * Constructs a MecanumDriveOdometry object. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Pose2d initialPoseMeters) { + m_kinematics = kinematics; + m_poseMeters = initialPoseMeters; + m_previousAngle = initialPoseMeters.getRotation(); + } + + /** + * Constructs a MecanumDriveOdometry object with the default pose at the origin. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + */ + public MecanumDriveOdometry(MecanumDriveKinematics kinematics) { + this(kinematics, new Pose2d()); + } + + /** + * Resets the robot's position on the field. + * + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Pose2d poseMeters) { + m_poseMeters = poseMeters; + m_previousAngle = poseMeters.getRotation(); + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param currentTimeSeconds The current time in seconds. + * @param angle The angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * @return The new pose of the robot. + */ + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle, + MecanumDriveWheelSpeeds wheelSpeeds) { + double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; + m_prevTimeSeconds = currentTimeSeconds; + + var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds); + var newPose = m_poseMeters.exp( + new Twist2d(chassisState.vxMetersPerSecond * period, + chassisState.vyMetersPerSecond * period, + angle.minus(m_previousAngle).getRadians())); + + m_previousAngle = angle; + m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + return m_poseMeters; + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method automatically calculates the + * current time to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param angle THe angle of the robot. + * @param wheelSpeeds The current wheel speeds. + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d angle, + MecanumDriveWheelSpeeds wheelSpeeds) { + return updateWithTime(System.currentTimeMillis() / 1000.0, angle, + wheelSpeeds); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java new file mode 100644 index 0000000000..54ae521098 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import java.util.stream.DoubleStream; + +@SuppressWarnings("MemberName") +public class MecanumDriveWheelSpeeds { + /** + * Speed of the front left wheel. + */ + public double frontLeftMetersPerSecond; + + /** + * Speed of the front right wheel. + */ + public double frontRightMetersPerSecond; + + /** + * Speed of the rear left wheel. + */ + public double rearLeftMetersPerSecond; + + /** + * Speed of the rear right wheel. + */ + public double rearRightMetersPerSecond; + + /** + * Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. + */ + public MecanumDriveWheelSpeeds() { + } + + /** + * Constructs a MecanumDriveWheelSpeeds. + * + * @param frontLeftMetersPerSecond Speed of the front left wheel. + * @param frontRightMetersPerSecond Speed of the front right wheel. + * @param rearLeftMetersPerSecond Speed of the rear left wheel. + * @param rearRightMetersPerSecond Speed of the rear right wheel. + */ + public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond, + double frontRightMetersPerSecond, + double rearLeftMetersPerSecond, + double rearRightMetersPerSecond) { + this.frontLeftMetersPerSecond = frontLeftMetersPerSecond; + this.frontRightMetersPerSecond = frontRightMetersPerSecond; + this.rearLeftMetersPerSecond = rearLeftMetersPerSecond; + this.rearRightMetersPerSecond = rearRightMetersPerSecond; + } + + /** + * Normalizes the wheel speeds using some max attainable speed. Sometimes, + * after inverse kinematics, the requested speed from a/several modules may be + * above the max attainable speed for the driving motor on that module. To fix + * this issue, one can "normalize" all the wheel speeds to make sure that all + * requested module speeds are below the absolute threshold, while maintaining + * the ratio of speeds between modules. + * + * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. + */ + public void normalize(double attainableMaxSpeedMetersPerSecond) { + double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond, + frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond) + .max().getAsDouble(); + + if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { + frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java new file mode 100644 index 0000000000..ab0700920f --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java @@ -0,0 +1,196 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import java.util.Arrays; +import java.util.Collections; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual module states (speed and angle). + * + *

The inverse kinematics (converting from a desired chassis velocity to + * individual module states) uses the relative locations of the modules with + * respect to the center of rotation. The center of rotation for inverse + * kinematics is also variable. This means that you can set your set your center + * of rotation in a corner of the robot to perform special evasion manuevers. + * + *

Forward kinematics (converting an array of module states into the overall + * chassis motion) is performs the exact opposite of what inverse kinematics + * does. Since this is an overdetermined system (more equations than variables), + * we use a least-squares approximation. + * + *

The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] + * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then + * multiply by [moduleStates] to get our chassis speeds. + * + *

Forward kinematics is also used for odometry -- determining the position of + * the robot on the field using encoders and a gyro. + */ +public class SwerveDriveKinematics { + private final SimpleMatrix m_inverseKinematics; + private final SimpleMatrix m_forwardKinematics; + + private final int m_numModules; + private final Translation2d[] m_modules; + private Translation2d m_prevCoR = new Translation2d(); + + /** + * Constructs a swerve drive kinematics object. This takes in a variable + * number of wheel locations as Translation2ds. The order in which you pass in + * the wheel locations is the same order that you will recieve the module + * states when performing inverse kinematics. It is also expected that you + * pass in the module states in the same order when calling the forward + * kinematics methods. + * + * @param wheelsMetersPerSecond The locations of the wheels relative to the physical center + * of the robot. + */ + public SwerveDriveKinematics(Translation2d... wheelsMetersPerSecond) { + if (wheelsMetersPerSecond.length < 2) { + throw new IllegalArgumentException("A swerve drive requires at least two modules"); + } + m_numModules = wheelsMetersPerSecond.length; + m_modules = Arrays.copyOf(wheelsMetersPerSecond, m_numModules); + m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); + + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); + m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); + } + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + } + + /** + * Performs inverse kinematics to return the module states from a desired + * chassis velocity. This method is often used to convert joystick values into + * module speeds and angles. + * + *

This function also supports variable centers of rotation. During normal + * operations, the center of rotation is usually the same as the physical + * center of the robot; therefore, the argument is defaulted to that use case. + * However, if you wish to change the center of rotation for evasive + * manuevers, vision alignment, or for any other use case, you can do so. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotationMeters The center of rotation. For example, if you set the + * center of rotation at one corner of the robot and provide + * a chassis speed that only has a dtheta component, the robot + * will rotate around that corner. + * @return An array containing the module states. Use caution because these + * module states are not normalized. Sometimes, a user input may cause one of + * the module speeds to go above the attainable max velocity. Use the + * {@link #normalizeWheelSpeeds(SwerveModuleState[], double) normalizeWheelSpeeds} + * function to rectify this issue. + */ + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, + Translation2d centerOfRotationMeters) { + if (centerOfRotationMeters.getX() != m_prevCoR.getX() + || centerOfRotationMeters.getY() != m_prevCoR.getY()) { + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, + -m_modules[i].getY() + centerOfRotationMeters.getY()); + m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, + +m_modules[i].getX() - centerOfRotationMeters.getX()); + } + m_prevCoR = centerOfRotationMeters; + } + + var chassisSpeedsVector = new SimpleMatrix(3, 1); + chassisSpeedsVector.setColumn(0, 0, + chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); + + var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); + SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules]; + + for (int i = 0; i < m_numModules; i++) { + double x = moduleStatesMatrix.get(i * 2, 0); + double y = moduleStatesMatrix.get(i * 2 + 1, 0); + + double speed = Math.hypot(x, y); + Rotation2d angle = new Rotation2d(x, y); + + moduleStates[i] = new SwerveModuleState(speed, angle); + } + + return moduleStates; + } + + /** + * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)} + * toSwerveModuleStates for more information. + * + * @param chassisSpeeds The desired chassis speed. + * @return An array containing the module states. + */ + public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) { + return toSwerveModuleStates(chassisSpeeds, new Translation2d()); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the + * given module states. This method is often used for odometry -- determining + * the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param wheelStates The state of the modules (as a SwerveModuleState type) + * as measured from respective encoders and gyros. The order of the swerve + * module states should be same as passed into the constructor of this class. + * @return The resulting chassis speed. + */ + ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) { + if (wheelStates.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor" + ); + } + var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = wheelStates[i]; + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); + moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + } + + var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); + return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0), + chassisSpeedsVector.get(2, 0)); + + } + + /** + * Normalizes the wheel speeds using some max attainable speed. Sometimes, + * after inverse kinematics, the requested speed from a/several modules may be + * above the max attainable speed for the driving motor on that module. To fix + * this issue, one can "normalize" all the wheel speeds to make sure that all + * requested module speeds are below the absolute threshold, while maintaining + * the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be + * mutated with the normalized speeds! + * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach. + */ + public static void normalizeWheelSpeeds(SwerveModuleState[] moduleStates, + double attainableMaxSpeedMetersPerSecond) { + double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond; + if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { + for (SwerveModuleState moduleState : moduleStates) { + moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed + * attainableMaxSpeedMetersPerSecond; + } + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java new file mode 100644 index 0000000000..ac28c209f1 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Twist2d; + +/** + * Class for swerve drive odometry. Odometry allows you to track the robot's + * position on the field over a course of a match using readings from your + * swerve drive encoders and swerve azimuth encoders. + * + *

Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + */ +public class SwerveDriveOdometry { + private final SwerveDriveKinematics m_kinematics; + private Pose2d m_poseMeters; + private double m_prevTimeSeconds = -1; + + private Rotation2d m_previousAngle; + + /** + * Constructs a SwerveDriveOdometry object. + * + * @param kinematics The swerve drive kinematics for your drivetrain. + * @param initialPose The starting position of the robot on the field. + */ + public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Pose2d initialPose) { + m_kinematics = kinematics; + m_poseMeters = initialPose; + m_previousAngle = initialPose.getRotation(); + } + + /** + * Constructs a SwerveDriveOdometry object with the default pose at the origin. + * + * @param kinematics The swerve drive kinematics for your drivetrain. + */ + public SwerveDriveOdometry(SwerveDriveKinematics kinematics) { + this(kinematics, new Pose2d()); + } + + /** + * Resets the robot's position on the field. + * + * @param pose The position on the field that your robot is at. + */ + public void resetPosition(Pose2d pose) { + m_poseMeters = pose; + m_previousAngle = pose.getRotation(); + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the + * angular rate that is calculated from forward kinematics. + * + * @param currentTimeSeconds The current time in seconds. + * @param angle The angle of the robot. + * @param moduleStates The current state of all swerve modules. Please provide + * the states in the same order in which you instantiated your + * SwerveDriveKinematics. + * @return The new pose of the robot. + */ + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle, + SwerveModuleState... moduleStates) { + double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; + m_prevTimeSeconds = currentTimeSeconds; + + var chassisState = m_kinematics.toChassisSpeeds(moduleStates); + var newPose = m_poseMeters.exp( + new Twist2d(chassisState.vxMetersPerSecond * period, + chassisState.vyMetersPerSecond * period, + angle.minus(m_previousAngle).getRadians())); + + m_previousAngle = angle; + m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + + return m_poseMeters; + } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method automatically calculates the + * current time to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the angular + * rate that is calculated from forward kinematics. + * + * @param angle The angle of the robot. + * @param moduleStates The current state of all swerve modules. Please provide + * the states in the same order in which you instantiated your + * SwerveDriveKinematics. + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d angle, SwerveModuleState... moduleStates) { + return updateWithTime(System.currentTimeMillis() / 1000.0, angle, moduleStates); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java new file mode 100644 index 0000000000..528776f5cc --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +/** + * Represents the state of one swerve module. + */ +@SuppressWarnings("MemberName") +public class SwerveModuleState implements Comparable { + + /** + * Speed of the wheel of the module. + */ + public double speedMetersPerSecond; + + /** + * Angle of the module. + */ + public Rotation2d angle = Rotation2d.fromDegrees(0); + + /** + * Constructs a SwerveModuleState with zeros for speed and angle. + */ + public SwerveModuleState() { + } + + /** + * Constructs a SwerveModuleState. + * + * @param speedMetersPerSecond The speed of the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) { + this.speedMetersPerSecond = speedMetersPerSecond; + this.angle = angle; + } + + /** + * Compares two swerve module states. One swerve module is "greater" than the other if its speed + * is higher than the other. + * + * @param o The other swerve module. + * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. + */ + @Override + @SuppressWarnings("ParameterName") + public int compareTo(SwerveModuleState o) { + return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java new file mode 100644 index 0000000000..729d7b84c4 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class ChassisSpeedsTest { + private static final double kEpsilon = 1E-9; + + @Test + void testFieldRelativeConstruction() { + final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + 1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0) + ); + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon) + ); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java new file mode 100644 index 0000000000..e484eabc25 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class DifferentialDriveKinematicsTest { + private static final double kEpsilon = 1E-9; + private final DifferentialDriveKinematics m_kinematics + = new DifferentialDriveKinematics(0.381 * 2); + + @Test + void testInverseKinematicsForZeros() { + var chassisSpeeds = new ChassisSpeeds(); + var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + + assertAll( + () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon) + ); + } + + @Test + void testForwardKinematicsForZeros() { + var wheelSpeeds = new DifferentialDriveWheelSpeeds(); + var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon) + ); + } + + @Test + void testInverseKinematicsForStraightLine() { + var chassisSpeeds = new ChassisSpeeds(3, 0, 0); + var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + + assertAll( + () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon), + () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon) + ); + } + + @Test + void testForwardKinematicsForStraightLine() { + var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3); + var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + + assertAll( + () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon) + ); + } + + @Test + void testInverseKinematicsForRotateInPlace() { + var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI); + var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + + assertAll( + () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon), + () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon) + ); + } + + @Test + void testForwardKinematicsForRotateInPlace() { + var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI); + var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon) + ); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java new file mode 100644 index 0000000000..4562ed2603 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class DifferentialDriveOdometryTest { + private static final double kEpsilon = 1E-9; + private final DifferentialDriveKinematics m_kinematics + = new DifferentialDriveKinematics(0.381 * 2); + private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics); + + @Test + void testOneIteration() { + m_odometry.resetPosition(new Pose2d()); + var speeds = new DifferentialDriveWheelSpeeds(0.02, 0.02); + m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds()); + var pose = m_odometry.updateWithTime(1.0, new Rotation2d(), speeds); + + assertAll( + () -> assertEquals(0.02, pose.getTranslation().getX(), kEpsilon), + () -> assertEquals(0.00, pose.getTranslation().getY(), kEpsilon), + () -> assertEquals(0.00, pose.getRotation().getRadians(), kEpsilon) + ); + } + + @Test + void testQuarterCircle() { + m_odometry.resetPosition(new Pose2d()); + var speeds = new DifferentialDriveWheelSpeeds(0.0, 5 * Math.PI); + m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds()); + var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), speeds); + + assertAll( + () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon), + () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon), + () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon) + ); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java new file mode 100644 index 0000000000..75b6f448b7 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java @@ -0,0 +1,263 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Translation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +@SuppressWarnings("PMD.TooManyMethods") +class MecanumDriveKinematicsTest { + private static final double kEpsilon = 1E-9; + + private final Translation2d m_fl = new Translation2d(12, 12); + private final Translation2d m_fr = new Translation2d(12, -12); + private final Translation2d m_bl = new Translation2d(-12, 12); + private final Translation2d m_br = new Translation2d(-12, -12); + + private final MecanumDriveKinematics m_kinematics = + new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br); + + @Test + void testStraightLineInverseKinematics() { + ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0); + var moduleStates = m_kinematics.toWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 + */ + + assertAll( + () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1) + ); + } + + @Test + void testStraightLineForwardKinematicsKinematics() { + + var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); + var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]] + */ + + assertAll( + () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testStrafeInverseKinematics() { + ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0); + var moduleStates = m_kinematics.toWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427 + */ + + assertAll( + () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1) + ); + } + + @Test + void testStrafeForwardKinematicsKinematics() { + + var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427); + var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]] + */ + + assertAll( + () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testRotationInverseKinematics() { + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); + var moduleStates = m_kinematics.toWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 + */ + + assertAll( + () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1) + ); + } + + @Test + void testRotationForwardKinematicsKinematics() { + var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191); + var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should be [[0][0][2pi]] + */ + + assertAll( + () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testMixedTranslationRotationInverseKinematics() { + ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1); + var moduleStates = m_kinematics.toWheelSpeeds(speeds); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 + */ + + assertAll( + () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1) + ); + } + + @Test + void testMixedTranslationRotationForwardKinematicsKinematics() { + var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26); + var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from wheel + velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be [[2][3][1]] + */ + + assertAll( + () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testOffCenterRotationInverseKinematics() { + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1); + var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125 + */ + + assertAll( + () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1) + ); + } + + @Test + void testOffCenterRotationForwardKinematicsKinematics() { + var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941); + var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from the wheel + velocities should be [[12][-12][1]] + */ + + assertAll( + () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testOffCenterTranslationRotationInverseKinematics() { + ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1); + var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl); + + /* + By equation (13.12) of the state-space-guide, the wheel speeds should + be as follows: + velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446 + */ + + assertAll( + () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1) + ); + } + + @Test + void testOffCenterRotationTranslationForwardKinematicsKinematics() { + + var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06); + var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); + + /* + By equation (13.13) of the state-space-guide, the chassis motion from the wheel + velocities should be [[17][-10][1]] + */ + + assertAll( + () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testNormalize() { + var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7); + wheelSpeeds.normalize(5.5); + + double factor = 5.5 / 7.0; + + assertAll( + () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon), + () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon), + () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon), + () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon) + ); + } + +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java new file mode 100644 index 0000000000..04accebe58 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class MecanumDriveOdometryTest { + private final Translation2d m_fl = new Translation2d(12, 12); + private final Translation2d m_fr = new Translation2d(12, -12); + private final Translation2d m_bl = new Translation2d(-12, 12); + private final Translation2d m_br = new Translation2d(-12, -12); + + private final MecanumDriveKinematics m_kinematics = + new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br); + + private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics); + + @Test + void testMultipleConsecutiveUpdates() { + var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); + + m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds); + var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds); + + assertAll( + () -> assertEquals(secondPose.getTranslation().getX(), 0.0, 0.01), + () -> assertEquals(secondPose.getTranslation().getY(), 0.0, 0.01), + () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01) + ); + } + + @Test + void testTwoIterations() { + // 5 units/sec in the x axis (forward) + final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); + + m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds()); + var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); + + assertAll( + () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01), + () -> assertEquals(0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01) + ); + } + + @Test + void test90degreeTurn() { + // This is a 90 degree turn about the point between front left and rear left wheels + // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946 + final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986); + + m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds()); + final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); + + assertAll( + () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01), + () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01) + ); + } + +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java new file mode 100644 index 0000000000..f506436a40 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java @@ -0,0 +1,263 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +@SuppressWarnings("PMD.TooManyMethods") +class SwerveDriveKinematicsTest { + private static final double kEpsilon = 1E-9; + + private final Translation2d m_fl = new Translation2d(12, 12); + private final Translation2d m_fr = new Translation2d(12, -12); + private final Translation2d m_bl = new Translation2d(-12, 12); + private final Translation2d m_br = new Translation2d(-12, -12); + + private final SwerveDriveKinematics m_kinematics = + new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br); + + @Test + void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line + + ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0); + var moduleStates = m_kinematics.toSwerveModuleStates(speeds); + + assertAll( + () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon), + () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon), + () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon), + () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon) + ); + } + + @Test + void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line + SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0)); + var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state); + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon) + ); + } + + @Test + void testStraightStrafeInverseKinematics() { + + ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0); + var moduleStates = m_kinematics.toSwerveModuleStates(speeds); + + assertAll( + () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon), + () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon) + ); + } + + @Test + void testStraightStrafeForwardKinematics() { + SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0)); + var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state); + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon) + ); + } + + @Test + void testTurnInPlaceInverseKinematics() { + + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); + var moduleStates = m_kinematics.toSwerveModuleStates(speeds); + + /* + The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius + the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels + trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second, + our wheels must trace out 1 rotation (or 106.63 inches) per second. + */ + + assertAll( + () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1), + () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1), + () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1), + () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1), + () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon), + () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon), + () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon), + () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon) + ); + } + + @Test + void testTurnInPlaceForwardKinematics() { + SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135)); + SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45)); + SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135)); + SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45)); + + var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testOffCenterCORRotationInverseKinematics() { + + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); + var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl); + + /* + This one is a bit trickier. Because we are rotating about the front-left wheel, + it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel + an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with + radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel + should be pointing straight forward, the back-left wheel should be pointing straight right, + and the back-right wheel should be at a -45 degree angle + */ + + assertAll( + () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1), + () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1), + () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1), + () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1), + () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), + () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon), + () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon), + () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon) + ); + } + + @Test + void testOffCenterCORRotationForwardKinematics() { + SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)); + SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0)); + SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90)); + SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45)); + + var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); + + /* + We already know that our omega should be 2pi from the previous test. Next, we need to determine + the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, + we know that vx and vy must be the same. Furthermore, we know that the center of mass makes + a full revolution about the center of revolution once every second. Therefore, the center of + mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are + 1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398. + */ + + assertAll( + () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1), + () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1), + () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1) + ); + } + + private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual, + SwerveModuleState tolerance) { + assertAll( + () -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond, + tolerance.speedMetersPerSecond), + () -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(), + tolerance.angle.getDegrees()) + ); + } + + /** + * Test the rotation of the robot about a non-central point with + * both linear and angular velocities. + */ + @Test + void testOffCenterCORRotationAndTranslationInverseKinematics() { + + ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5); + var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0)); + + // By equation (13.14) from state-space guide, our wheels/angles will be as follows, + // (+-1 degree or speed): + SwerveModuleState[] expectedStates = new SwerveModuleState[]{ + new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)), + new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)), + new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)), + new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56)) + }; + var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1)); + + for (int i = 0; i < expectedStates.length; i++) { + assertModuleState(expectedStates[i], moduleStates[i], stateTolerance); + } + } + + @Test + void testOffCenterCORRotationAndTranslationForwardKinematics() { + SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)); + SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)); + SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)); + SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56)); + + var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); + + /* + From equation (13.17), we know that chassis motion is th dot product of the + pseudoinverse of the inverseKinematics matrix (with the center of rotation at + (0,0) -- we don't want the motion of the center of rotation, we want it of + the center of the robot). These above SwerveModuleStates are known to be from + a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been + calculated using Numpy's linalg.pinv function. + */ + + assertAll( + () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1), + () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1), + () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1) + ); + } + + @Test + void testNormalize() { + SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d()); + SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d()); + SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d()); + SwerveModuleState br = new SwerveModuleState(7, new Rotation2d()); + + SwerveModuleState[] arr = {fl, fr, bl, br}; + SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5); + + double factor = 5.5 / 7.0; + + assertAll( + () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon), + () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon), + () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon), + () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon) + ); + } + +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java new file mode 100644 index 0000000000..c945b8bd4e --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class SwerveDriveOdometryTest { + private final Translation2d m_fl = new Translation2d(12, 12); + private final Translation2d m_fr = new Translation2d(12, -12); + private final Translation2d m_bl = new Translation2d(-12, 12); + private final Translation2d m_br = new Translation2d(-12, -12); + + private final SwerveDriveKinematics m_kinematics = + new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br); + + private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics); + + @Test + void testTwoIterations() { + // 5 units/sec in the x axis (forward) + final SwerveModuleState[] wheelSpeeds = { + new SwerveModuleState(5, Rotation2d.fromDegrees(0)), + new SwerveModuleState(5, Rotation2d.fromDegrees(0)), + new SwerveModuleState(5, Rotation2d.fromDegrees(0)), + new SwerveModuleState(5, Rotation2d.fromDegrees(0)) + }; + + m_odometry.updateWithTime(0.0, new Rotation2d(), + new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState()); + var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); + + assertAll( + () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01), + () -> assertEquals(0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01) + ); + } + + @Test + void test90degreeTurn() { + // This is a 90 degree turn about the point between front left and rear left wheels + // Module 0: speed 18.84955592153876 angle 90.0 + // Module 1: speed 42.14888838624436 angle 26.565051177077986 + // Module 2: speed 18.84955592153876 angle -90.0 + // Module 3: speed 42.14888838624436 angle -26.565051177077986 + + final SwerveModuleState[] wheelSpeeds = { + new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)), + new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)), + new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)), + new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565)) + }; + final var zero = new SwerveModuleState(); + + m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero); + final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); + + assertAll( + () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01), + () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01) + ); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java new file mode 100644 index 0000000000..9f33156682 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.differentialdrivebot; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; + +/** + * Represents a differential drive style drivetrain. + */ +public class Drivetrain { + public static final double kMaxSpeed = 3.0; // meters per second + public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second + + private static final double kTrackWidth = 0.381 * 2; // meters + private static final double kWheelRadius = 0.0508; // meters + private static final int kEncoderResolution = 4096; + + private final Spark m_leftMaster = new Spark(1); + private final Spark m_leftFollower = new Spark(2); + private final Spark m_rightMaster = new Spark(3); + private final Spark m_rightFollower = new Spark(4); + + private final Encoder m_leftEncoder = new Encoder(0, 1); + private final Encoder m_rightEncoder = new Encoder(0, 1); + + private final SpeedControllerGroup m_leftGroup + = new SpeedControllerGroup(m_leftMaster, m_leftFollower); + private final SpeedControllerGroup m_rightGroup + = new SpeedControllerGroup(m_rightMaster, m_rightFollower); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final PIDController m_leftPIDController = new PIDController(1, 0, 0); + private final PIDController m_rightPIDController = new PIDController(1, 0, 0); + + private final DifferentialDriveKinematics m_kinematics + = new DifferentialDriveKinematics(kTrackWidth); + + private final DifferentialDriveOdometry m_odometry + = new DifferentialDriveOdometry(m_kinematics); + + /** + * Constructs a differential drive object. + * Sets the encoder distance per pulse and resets the gyro. + */ + public Drivetrain() { + m_gyro.reset(); + + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + } + + /** + * Returns the angle of the robot as a Rotation2d. + * + * @return The angle of the robot. + */ + public Rotation2d getAngle() { + // Negating the angle because WPILib gyros are CW positive. + return Rotation2d.fromDegrees(-m_gyro.getAngle()); + } + + /** + * Returns the current wheel speeds. + * + * @return The current wheel speeds. + */ + public DifferentialDriveWheelSpeeds getCurrentSpeeds() { + return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); + } + + /** + * Sets the desired wheel speeds. + * + * @param speeds The desired wheel speeds. + */ + public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { + double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), + speeds.leftMetersPerSecond); + double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), + speeds.rightMetersPerSecond); + m_leftGroup.set(leftOutput); + m_rightGroup.set(rightOutput); + } + + /** + * Drives the robot with the given linear velocity and angular velocity. + * + * @param xSpeed Linear velocity in m/s. + * @param rot Angular velocity in rad/s. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double rot) { + var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot)); + setSpeeds(wheelSpeeds); + } + + /** + * Updates the field-relative position. + */ + public void updateOdometry() { + m_odometry.update(getAngle(), getCurrentSpeeds()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java new file mode 100644 index 0000000000..1f2b319843 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.differentialdrivebot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java new file mode 100644 index 0000000000..10df8b6f39 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.differentialdrivebot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; + +import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxAngularSpeed; +import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxSpeed; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + private final Drivetrain m_drive = new Drivetrain(); + + @Override + public void autonomousPeriodic() { + teleopPeriodic(); + m_drive.updateOdometry(); + } + + @Override + public void teleopPeriodic() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed; + + m_drive.drive(xSpeed, rot); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index c062fd9967..0e5d67a614 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -343,5 +343,35 @@ ], "foldername": "gyrodrivecommands", "mainclass": "Main" + }, + { + "name": "SwerveBot", + "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.", + "tags": [ + "SwerveBot" + ], + "foldername": "swervebot", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "MecanumBot", + "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.", + "tags": [ + "MecanumBot" + ], + "foldername": "mecanumbot", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "DifferentialDriveBot", + "description": "An example program for a differential drive that uses differential drive kinematics and odometry.", + "tags": [ + "MecanumBot" + ], + "foldername": "differentialdrivebot", + "gradlebase": "java", + "mainclass": "Main" } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java new file mode 100644 index 0000000000..29a6f49eb0 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -0,0 +1,138 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumbot; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; + +/** + * Represents a mecanum drive style drivetrain. + */ +@SuppressWarnings("PMD.TooManyFields") +public class Drivetrain { + public static final double kMaxSpeed = 3.0; // 3 meters per second + public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second + + private final Spark m_frontLeftMotor = new Spark(1); + private final Spark m_frontRightMotor = new Spark(2); + private final Spark m_backLeftMotor = new Spark(3); + private final Spark m_backRightMotor = new Spark(4); + + private final Encoder m_frontLeftEncoder = new Encoder(0, 1); + private final Encoder m_frontRightEncoder = new Encoder(0, 1); + private final Encoder m_backLeftEncoder = new Encoder(0, 1); + private final Encoder m_backRightEncoder = new Encoder(0, 1); + + private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0); + private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0); + private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0); + private final PIDController m_backRightPIDController = new PIDController(1, 0, 0); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + ); + + private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics); + + /** + * Constructs a MecanumDrive and resets the gyro. + */ + public Drivetrain() { + m_gyro.reset(); + } + + /** + * Returns the angle of the robot as a Rotation2d. + * + * @return The angle of the robot. + */ + public Rotation2d getAngle() { + // Negating the angle because WPILib gyros are CW positive. + return Rotation2d.fromDegrees(-m_gyro.getAngle()); + } + + /** + * Returns the current state of the drivetrain. + * + * @return The current state of the drivetrain. + */ + public MecanumDriveWheelSpeeds getCurrentState() { + return new MecanumDriveWheelSpeeds( + m_frontLeftEncoder.getRate(), + m_frontRightEncoder.getRate(), + m_backLeftEncoder.getRate(), + m_backRightEncoder.getRate() + ); + } + + /** + * Set the desired speeds for each wheel. + * + * @param speeds The desired wheel speeds. + */ + public void setSpeeds(MecanumDriveWheelSpeeds speeds) { + final var frontLeftOutput = m_frontLeftPIDController.calculate( + m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond + ); + final var frontRightOutput = m_frontRightPIDController.calculate( + m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond + ); + final var backLeftOutput = m_backLeftPIDController.calculate( + m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond + ); + final var backRightOutput = m_backRightPIDController.calculate( + m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond + ); + + m_frontLeftMotor.set(frontLeftOutput); + m_frontRightMotor.set(frontRightOutput); + m_backLeftMotor.set(backLeftOutput); + m_backRightMotor.set(backRightOutput); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, getAngle() + ) : new ChassisSpeeds(xSpeed, ySpeed, rot) + ); + mecanumDriveWheelSpeeds.normalize(kMaxSpeed); + setSpeeds(mecanumDriveWheelSpeeds); + } + + /** + * Updates the field relative position of the robot. + */ + public void updateOdometry() { + m_odometry.update(getAngle(), getCurrentState()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java new file mode 100644 index 0000000000..27a4b322d2 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumbot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java new file mode 100644 index 0000000000..b7540d3c15 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumbot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; + +import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed; +import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + private final Drivetrain m_mecanum = new Drivetrain(); + + @Override + public void autonomousPeriodic() { + driveWithJoystick(false); + m_mecanum.updateOdometry(); + } + + @Override + public void teleopPeriodic() { + driveWithJoystick(true); + } + + private void driveWithJoystick(boolean fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed; + + m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java new file mode 100644 index 0000000000..0ec09f085b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervebot; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry; + +/** + * Represents a swerve drive style drivetrain. + */ +public class Drivetrain { + public static final double kMaxSpeed = 3.0; // 3 meters per second + public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second + + private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + private final SwerveModule m_frontLeft = new SwerveModule(1, 2); + private final SwerveModule m_frontRight = new SwerveModule(3, 4); + private final SwerveModule m_backLeft = new SwerveModule(5, 6); + private final SwerveModule m_backRight = new SwerveModule(7, 8); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + ); + + private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics); + + public Drivetrain() { + m_gyro.reset(); + } + + /** + * Returns the angle of the robot as a Rotation2d. + * + * @return The angle of the robot. + */ + public Rotation2d getAngle() { + // Negating the angle because WPILib gyros are CW positive. + return Rotation2d.fromDegrees(-m_gyro.getAngle()); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + var swerveModuleStates = m_kinematics.toSwerveModuleStates( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, getAngle()) + : new ChassisSpeeds(xSpeed, ySpeed, rot) + ); + SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + m_frontRight.setDesiredState(swerveModuleStates[1]); + m_backLeft.setDesiredState(swerveModuleStates[2]); + m_backRight.setDesiredState(swerveModuleStates[3]); + } + + /** + * Updates the field relative position of the robot. + */ + public void updateOdometry() { + m_odometry.update( + getAngle(), + m_frontLeft.getState(), + m_frontRight.getState(), + m_backLeft.getState(), + m_backRight.getState() + ); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java new file mode 100644 index 0000000000..52400e623d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervebot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java new file mode 100644 index 0000000000..ff9f28b26d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervebot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; + +import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxAngularSpeed; +import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxSpeed; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + private final Drivetrain m_swerve = new Drivetrain(); + + @Override + public void autonomousPeriodic() { + driveWithJoystick(false); + m_swerve.updateOdometry(); + } + + @Override + public void teleopPeriodic() { + driveWithJoystick(true); + } + + private void driveWithJoystick(boolean fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed; + + m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java new file mode 100644 index 0000000000..58a0c8d722 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -0,0 +1,91 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervebot; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +public class SwerveModule { + private static final double kWheelRadius = 0.0508; + private static final int kEncoderResolution = 4096; + + private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed; + private static final double kModuleMaxAngularAcceleration + = 2 * Math.PI; // radians per second squared + + private final Spark m_driveMotor; + private final Spark m_turningMotor; + + private final Encoder m_driveEncoder = new Encoder(0, 1); + private final Encoder m_turningEncoder = new Encoder(0, 1); + + private final PIDController m_drivePIDController = new PIDController(1, 0, 0); + + private final ProfiledPIDController m_turningPIDController + = new ProfiledPIDController(1, 0, 0, + new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration)); + + /** + * Constructs a SwerveModule. + * + * @param driveMotorChannel ID for the drive motor. + * @param turningMotorChannel ID for the turning motor. + */ + public SwerveModule(int driveMotorChannel, int turningMotorChannel) { + m_driveMotor = new Spark(driveMotorChannel); + m_turningMotor = new Spark(turningMotorChannel); + + // Set the distance per pulse for the drive encoder. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + + // Set the distance (in this case, angle) per pulse for the turning encoder. + // This is the the angle through an entire rotation (2 * wpi::math::pi) + // divided by the encoder resolution. + m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution); + + // Limit the PID Controller's input range between -pi and pi and set the input + // to be continuous. + m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get())); + } + + /** + * Sets the desired state for the module. + * + * @param state Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState state) { + // Calculate the drive output from the drive PID controller. + final var driveOutput = m_drivePIDController.calculate( + m_driveEncoder.getRate(), state.speedMetersPerSecond); + + // Calculate the turning motor output from the turning PID controller. + final var turnOutput = m_turningPIDController.calculate( + m_turningEncoder.get(), state.angle.getRadians() + ); + + // Calculate the turning motor output from the turning PID controller. + m_driveMotor.set(driveOutput); + m_turningMotor.set(turnOutput); + } +} diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Core b/wpiutil/src/main/native/eigeninclude/Eigen/Core index b1aa7887ec..bd892c103b 100644 --- a/wpiutil/src/main/native/eigeninclude/Eigen/Core +++ b/wpiutil/src/main/native/eigeninclude/Eigen/Core @@ -11,6 +11,10 @@ #ifndef EIGEN_CORE_H #define EIGEN_CORE_H +#if __GNUC__ >= 9 +#pragma GCC diagnostic ignored "-Wdeprecated-copy" +#endif + // first thing Eigen does: stop the compiler from committing suicide #include "src/Core/util/DisableStupidWarnings.h" diff --git a/wpiutil/src/main/native/include/units/units.h b/wpiutil/src/main/native/include/units/units.h index 49b4cecb01..d755446ab7 100644 --- a/wpiutil/src/main/native/include/units/units.h +++ b/wpiutil/src/main/native/include/units/units.h @@ -4846,6 +4846,8 @@ namespace std using namespace units::literals; namespace units { +using namespace acceleration; +using namespace angular_velocity; using namespace length; using namespace time; using namespace velocity;