Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
8e2a7fd3069d32377cc147afbe69868c22d005b7
allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp

463 lines
14 KiB
C++
Raw Normal View History

[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
Include thirdparty libraries with angle brackets (#5578)
2023-08-28 15:13:34 -07:00
#include <gtest/gtest.h>
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
#include "frc/drive/DifferentialDrive.h"
[wpilibc] Rename SpeedControllerGroupTest.cpp (#4258) Also move motor controller test files into motorcontrol folder to match Java.
2022-05-21 16:20:26 -07:00
#include "motorcontrol/MockMotorController.h"
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
TEST(DifferentialDriveTest, ArcadeDriveIK) {
// Forward
auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(0.5, speeds.right);
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward
speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.5, speeds.right);
// Left turn (xSpeed with negative sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xSpeed with positive sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Right turn (xSpeed with negative sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xSpeed with positive sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
// Forward
auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward
speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
// Left turn (xSpeed with negative sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xSpeed with positive sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Right turn (xSpeed with negative sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xSpeed with positive sign)
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
TEST(DifferentialDriveTest, CurvatureDriveIK) {
// Forward
auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.75, speeds.right);
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.75, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
// Backward
speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-0.75, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-0.75, speeds.right);
}
TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
// Forward
auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward
speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
[wpilib] Check for signedness in ArcadeDriveIK() (#4028) If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the third quadrant instead of the first since +0.0 == -0.0. Also added tests for inverse kinematic functions, fixed some MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK() and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle that C++ already had. Fixes #4022.
2022-02-17 18:03:59 -08:00
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
TEST(DifferentialDriveTest, TankDriveIK) {
// Forward
auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, false);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.5, speeds.right);
// Backward
speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::TankDriveIK(-0.5, 1.0, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
}
TEST(DifferentialDriveTest, TankDriveIKSquared) {
// Forward
auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, true);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
// Backward
speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, true);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
}
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
TEST(DifferentialDriveTest, ArcadeDrive) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::DifferentialDrive drive{left, right};
drive.SetDeadband(0.0);
// Forward
drive.ArcadeDrive(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(0.5, 0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(0.5, right.Get());
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(0.5, -0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward
drive.ArcadeDrive(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(-0.5, 0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(-0.5, -0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-0.5, right.Get());
}
TEST(DifferentialDriveTest, ArcadeDriveSquared) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::DifferentialDrive drive{left, right};
drive.SetDeadband(0.0);
// Forward
drive.ArcadeDrive(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(0.5, 0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(0.5, -0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward
drive.ArcadeDrive(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(-0.5, 0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.ArcadeDrive(-0.5, -0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
}
TEST(DifferentialDriveTest, CurvatureDrive) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::DifferentialDrive drive{left, right};
drive.SetDeadband(0.0);
// Forward
drive.CurvatureDrive(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(0.5, 0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(0.75, right.Get());
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(0.5, -0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.75, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
// Backward
drive.CurvatureDrive(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(-0.5, 0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(-0.75, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(-0.5, -0.5, false);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(-0.75, right.Get());
}
TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::DifferentialDrive drive{left, right};
drive.SetDeadband(0.0);
// Forward
drive.CurvatureDrive(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(0.5, 0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(0.5, -0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward
drive.CurvatureDrive(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(-0.5, 0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward right turn
[wpilib] Make drive classes follow NWU axes convention (#4079) All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
2022-10-27 21:59:11 -07:00
drive.CurvatureDrive(-0.5, -0.5, true);
[wpilib] DifferentialDrive: Remove right side inversion (#3340) Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
2021-05-21 22:34:16 -07:00
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
}
TEST(DifferentialDriveTest, TankDrive) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::DifferentialDrive drive{left, right};
drive.SetDeadband(0.0);
// Forward
drive.TankDrive(1.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
drive.TankDrive(0.5, 1.0, false);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward right turn
drive.TankDrive(1.0, 0.5, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.5, right.Get());
// Backward
drive.TankDrive(-1.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
drive.TankDrive(-0.5, -1.0, false);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward right turn
drive.TankDrive(-0.5, 1.0, false);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
}
TEST(DifferentialDriveTest, TankDriveSquared) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::DifferentialDrive drive{left, right};
drive.SetDeadband(0.0);
// Forward
drive.TankDrive(1.0, 1.0, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
drive.TankDrive(0.5, 1.0, true);
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward right turn
drive.TankDrive(1.0, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
// Backward
drive.TankDrive(-1.0, -1.0, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
drive.TankDrive(-0.5, -1.0, true);
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward right turn
drive.TankDrive(-1.0, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
}
Reference in New Issue Copy Permalink