mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
192 lines
8.3 KiB
C++
192 lines
8.3 KiB
C++
|
/*----------------------------------------------------------------------------*/
|
||
|
|
/* Copyright (c) 2014-2017 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 "Drive/DifferentialDrive.h"
|
||
|
|
#include "Drive/MecanumDrive.h"
|
||
|
|
#include "MockSpeedController.h"
|
||
|
|
#include "RobotDrive.h"
|
||
|
|
#include "TestBench.h"
|
||
|
|
#include "gtest/gtest.h"
|
||
|
|
|
||
|
|
using namespace frc;
|
||
|
|
|
||
|
|
class RobotDriveTest : public testing::Test {
|
||
|
|
protected:
|
||
|
|
MockSpeedController m_rdFrontLeft;
|
||
|
|
MockSpeedController m_rdRearLeft;
|
||
|
|
MockSpeedController m_rdFrontRight;
|
||
|
|
MockSpeedController m_rdRearRight;
|
||
|
|
MockSpeedController m_frontLeft;
|
||
|
|
MockSpeedController m_rearLeft;
|
||
|
|
MockSpeedController m_frontRight;
|
||
|
|
MockSpeedController m_rearRight;
|
||
|
|
frc::RobotDrive m_robotDrive{m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight,
|
||
|
|
m_rdRearRight};
|
||
|
|
frc::DifferentialDrive m_differentialDrive{m_frontLeft, m_frontRight};
|
||
|
|
frc::MecanumDrive m_mecanumDrive{m_frontLeft, m_rearLeft, m_frontRight,
|
||
|
|
m_rearRight};
|
||
|
|
|
||
|
|
double m_testJoystickValues[9] = {-1.0, -0.9, -0.5, -0.01, 0.0,
|
||
|
|
0.01, 0.5, 0.9, 1.0};
|
||
|
|
double m_testGyroValues[19] = {0, 45, 90, 135, 180, 225, 270,
|
||
|
|
305, 360, 540, -45, -90, -135, -180,
|
||
|
|
-225, -270, -305, -360, -540};
|
||
|
|
};
|
||
|
|
|
||
|
|
TEST_F(RobotDriveTest, TankDrive) {
|
||
|
|
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||
|
|
double leftJoystick, rightJoystick;
|
||
|
|
m_differentialDrive.SetDeadband(0.0);
|
||
|
|
m_differentialDrive.SetSafetyEnabled(false);
|
||
|
|
m_mecanumDrive.SetSafetyEnabled(false);
|
||
|
|
m_robotDrive.SetSafetyEnabled(false);
|
||
|
|
for (int i = 0; i < joystickSize; i++) {
|
||
|
|
for (int j = 0; j < joystickSize; j++) {
|
||
|
|
leftJoystick = m_testJoystickValues[i];
|
||
|
|
rightJoystick = m_testJoystickValues[j];
|
||
|
|
m_robotDrive.TankDrive(leftJoystick, rightJoystick, false);
|
||
|
|
m_differentialDrive.TankDrive(leftJoystick, rightJoystick, false);
|
||
|
|
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||
|
|
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(RobotDriveTest, TankDriveSquared) {
|
||
|
|
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||
|
|
double leftJoystick, rightJoystick;
|
||
|
|
m_differentialDrive.SetDeadband(0.0);
|
||
|
|
m_differentialDrive.SetSafetyEnabled(false);
|
||
|
|
m_mecanumDrive.SetSafetyEnabled(false);
|
||
|
|
m_robotDrive.SetSafetyEnabled(false);
|
||
|
|
for (int i = 0; i < joystickSize; i++) {
|
||
|
|
for (int j = 0; j < joystickSize; j++) {
|
||
|
|
leftJoystick = m_testJoystickValues[i];
|
||
|
|
rightJoystick = m_testJoystickValues[j];
|
||
|
|
m_robotDrive.TankDrive(leftJoystick, rightJoystick, true);
|
||
|
|
m_differentialDrive.TankDrive(leftJoystick, rightJoystick, true);
|
||
|
|
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||
|
|
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(RobotDriveTest, ArcadeDriveSquared) {
|
||
|
|
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||
|
|
double moveJoystick, rotateJoystick;
|
||
|
|
m_differentialDrive.SetDeadband(0.0);
|
||
|
|
m_differentialDrive.SetSafetyEnabled(false);
|
||
|
|
m_mecanumDrive.SetSafetyEnabled(false);
|
||
|
|
m_robotDrive.SetSafetyEnabled(false);
|
||
|
|
for (int i = 0; i < joystickSize; i++) {
|
||
|
|
for (int j = 0; j < joystickSize; j++) {
|
||
|
|
moveJoystick = m_testJoystickValues[i];
|
||
|
|
rotateJoystick = m_testJoystickValues[j];
|
||
|
|
m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, true);
|
||
|
|
m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, true);
|
||
|
|
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||
|
|
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(RobotDriveTest, ArcadeDrive) {
|
||
|
|
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||
|
|
double moveJoystick, rotateJoystick;
|
||
|
|
m_differentialDrive.SetDeadband(0.0);
|
||
|
|
m_differentialDrive.SetSafetyEnabled(false);
|
||
|
|
m_mecanumDrive.SetSafetyEnabled(false);
|
||
|
|
m_robotDrive.SetSafetyEnabled(false);
|
||
|
|
for (int i = 0; i < joystickSize; i++) {
|
||
|
|
for (int j = 0; j < joystickSize; j++) {
|
||
|
|
moveJoystick = m_testJoystickValues[i];
|
||
|
|
rotateJoystick = m_testJoystickValues[j];
|
||
|
|
m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, false);
|
||
|
|
m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, false);
|
||
|
|
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||
|
|
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(RobotDriveTest, MecanumCartesian) {
|
||
|
|
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||
|
|
int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
|
||
|
|
double xJoystick, yJoystick, rotateJoystick, gyroValue;
|
||
|
|
m_mecanumDrive.SetDeadband(0.0);
|
||
|
|
m_mecanumDrive.SetSafetyEnabled(false);
|
||
|
|
m_differentialDrive.SetSafetyEnabled(false);
|
||
|
|
m_robotDrive.SetSafetyEnabled(false);
|
||
|
|
for (int i = 0; i < joystickSize; i++) {
|
||
|
|
for (int j = 0; j < joystickSize; j++) {
|
||
|
|
for (int k = 0; k < joystickSize; k++) {
|
||
|
|
for (int l = 0; l < gyroSize; l++) {
|
||
|
|
xJoystick = m_testJoystickValues[i];
|
||
|
|
yJoystick = m_testJoystickValues[j];
|
||
|
|
rotateJoystick = m_testJoystickValues[k];
|
||
|
|
gyroValue = m_testGyroValues[l];
|
||
|
|
m_robotDrive.MecanumDrive_Cartesian(xJoystick, yJoystick,
|
||
|
|
rotateJoystick, gyroValue);
|
||
|
|
m_mecanumDrive.DriveCartesian(xJoystick, -yJoystick, rotateJoystick,
|
||
|
|
-gyroValue);
|
||
|
|
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
|
||
|
|
<< "X: " << xJoystick << " Y: " << yJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||
|
|
ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
|
||
|
|
<< "X: " << xJoystick << " Y: " << yJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||
|
|
ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
|
||
|
|
<< "X: " << xJoystick << " Y: " << yJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||
|
|
ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
|
||
|
|
<< "X: " << xJoystick << " Y: " << yJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(RobotDriveTest, MecanumPolar) {
|
||
|
|
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||
|
|
int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
|
||
|
|
double magnitudeJoystick, directionJoystick, rotateJoystick;
|
||
|
|
m_mecanumDrive.SetDeadband(0.0);
|
||
|
|
m_mecanumDrive.SetSafetyEnabled(false);
|
||
|
|
m_differentialDrive.SetSafetyEnabled(false);
|
||
|
|
m_robotDrive.SetSafetyEnabled(false);
|
||
|
|
for (int i = 0; i < joystickSize; i++) {
|
||
|
|
for (int j = 0; j < gyroSize; j++) {
|
||
|
|
for (int k = 0; k < joystickSize; k++) {
|
||
|
|
magnitudeJoystick = m_testJoystickValues[i];
|
||
|
|
directionJoystick = m_testGyroValues[j];
|
||
|
|
rotateJoystick = m_testJoystickValues[k];
|
||
|
|
m_robotDrive.MecanumDrive_Polar(magnitudeJoystick, directionJoystick,
|
||
|
|
rotateJoystick);
|
||
|
|
m_mecanumDrive.DrivePolar(magnitudeJoystick, directionJoystick,
|
||
|
|
rotateJoystick);
|
||
|
|
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
|
||
|
|
<< "Magnitude: " << magnitudeJoystick
|
||
|
|
<< " Direction: " << directionJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick;
|
||
|
|
ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
|
||
|
|
<< "Magnitude: " << magnitudeJoystick
|
||
|
|
<< " Direction: " << directionJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick;
|
||
|
|
ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
|
||
|
|
<< "Magnitude: " << magnitudeJoystick
|
||
|
|
<< " Direction: " << directionJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick;
|
||
|
|
ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
|
||
|
|
<< "Magnitude: " << magnitudeJoystick
|
||
|
|
<< " Direction: " << directionJoystick
|
||
|
|
<< " Rotate: " << rotateJoystick;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|