Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add tests for equivilance of RobotDrive and DifferentialDrive/MecanumDrive (#732)

Browse Source 
Add documentation for how to get same results as RobotDrive and improve
RobotDrive documentation
This commit is contained in:
sciencewhiz
2017-11-29 21:41:00 -08:00
committed by Peter Johnson
parent e308dd28f3
commit cbd08a1e11
10 changed files with 484 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibc/src/main/native/cpp/Drive/RobotDriveBase.cpp
View File

@@ -20,11 +20,18 @@ using namespace frc;
RobotDriveBase::RobotDriveBase() { m_safetyHelper.SetSafetyEnabled(true); }
/**
* Change the default value for deadband scaling. The default value is
* 0.02. Values smaller then the deadband are set to 0, while values
* larger then the deadband are scaled from 0.0 to 1.0. See ApplyDeadband().
*
* @param deadband The deadband to set.
*/
void RobotDriveBase::SetDeadband(double deadband) { m_deadband = deadband; }
/**
* Configure the scaling factor for using RobotDrive with motor controllers in a
* mode other than PercentVbus.
* mode other than PercentVbus or to limit the maximum output.
*
* @param maxOutput Multiplied with the output percentage computed by the drive
* functions.

14
wpilibc/src/main/native/include/Drive/DifferentialDrive.h
View File

@@ -81,6 +81,20 @@ class SpeedController;
* The positive X axis points ahead, the positive Y axis points to the right,
* and the positive Z axis points down. Rotations follow the right-hand rule, so
* clockwise rotation around the Z axis is positive.
*
* Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
* so that the full range is still used. This deadband value can be changed
* with SetDeadband().
*
* <p>RobotDrive porting guide:
* <br>TankDrive(double, double, bool) is equivalent to
* RobotDrive#TankDrive(double, double, bool) if a deadband of 0 is used.
* <br>ArcadeDrive(double, double, bool) is equivalent to
* RobotDrive#ArcadeDrive(double, double, bool) if a deadband of 0 is used
* and the the rotation input is inverted eg ArcadeDrive(y, -rotation, false)
* <br>CurvatureDrive(double, double, bool) is similar in concept to
* RobotDrive#Drive(double, double) with the addition of a quick turn
* mode. However, it is not designed to give exactly the same response.
*/
class DifferentialDrive : public RobotDriveBase {
public:

17
wpilibc/src/main/native/include/Drive/MecanumDrive.h
View File

@@ -44,6 +44,23 @@ class SpeedController;
* The positive X axis points ahead, the positive Y axis points to the right,
* and the positive Z axis points down. Rotations follow the right-hand rule, so
* clockwise rotation around the Z axis is positive.
*
* Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
* so that the full range is still used. This deadband value can be changed
* with SetDeadband().
*
* RobotDrive porting guide:
* <br>In MecanumDrive, the right side speed controllers are automatically
* inverted, while in RobotDrive, no speed controllers are automatically
* inverted.
* <br>DriveCartesian(double, double, double, double) is equivalent to
* RobotDrive#MecanumDrive_Cartesian(double, double, double, double)
* if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted
* compared to RobotDrive (eg DriveCartesian(xSpeed, -ySpeed, zRotation,
* -gyroAngle).
* <br>DrivePolar(double, double, double) is equivalent to
* RobotDrive#MecanumDrive_Polar(double, double, double) if a
* deadband of 0 is used.
*/
class MecanumDrive : public RobotDriveBase {
public:

191
wpilibcIntegrationTests/src/FRCUserProgram/cpp/RobotDriveTest.cpp Normal file
View File

@@ -0,0 +1,191 @@
/*----------------------------------------------------------------------------*/
/* 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;
}
}
}
}

35
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
View File

@@ -200,7 +200,8 @@ public class RobotDrive implements MotorSafety {
/**
* Provide tank steering using the stored robot configuration. drive the robot using two joystick
* inputs. The Y-axis will be selected from each Joystick object.
* inputs. The Y-axis will be selected from each Joystick object. The calculated values will be
* squared to decrease sensitivity at low speeds.
*
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
@@ -229,7 +230,8 @@ public class RobotDrive implements MotorSafety {
/**
* Provide tank steering using the stored robot configuration. This function lets you pick the
* axis to be used on each Joystick object for the left and right sides of the robot.
* axis to be used on each Joystick object for the left and right sides of the robot. The
* calculated values will be squared to decrease sensitivity at low speeds.
*
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
@@ -292,7 +294,8 @@ public class RobotDrive implements MotorSafety {
/**
* Provide tank steering using the stored robot configuration. This function lets you directly
* provide joystick values from any source.
* provide joystick values from any source. The calculated values will be squared to decrease
* sensitivity at low speeds.
*
* @param leftValue The value of the left stick.
* @param rightValue The value of the right stick.
@@ -304,7 +307,8 @@ public class RobotDrive implements MotorSafety {
/**
* Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
* axis for the move value and the X axis for the rotate value. (Should add more information here
* regarding the way that arcade drive works.)
* regarding the way that arcade drive works.) The calculated values will be squared to decrease
* sensitivity at low speeds.
*
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be
* selected for forwards/backwards and the X-axis will be selected for
@@ -319,7 +323,8 @@ public class RobotDrive implements MotorSafety {
/**
* Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
* axis for the move value and the X axis for the rotate value. (Should add more information here
* regarding the way that arcade drive works.)
* regarding the way that arcade drive works.) The calculated values will be squared to decrease
* sensitivity at low speeds.
*
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
* for forwards/backwards and the X-axis will be selected for rotation rate.
@@ -350,7 +355,8 @@ public class RobotDrive implements MotorSafety {
/**
* Arcade drive implements single stick driving. Given two joystick instances and two axis,
* compute the values to send to either two or four motors.
* compute the values to send to either two or four motors. The calculated values will be
* squared to decrease sensitivity at low speeds.
*
* @param moveStick The Joystick object that represents the forward/backward direction
* @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
@@ -418,7 +424,8 @@ public class RobotDrive implements MotorSafety {
/**
* Arcade drive implements single stick driving. This function lets you directly provide
* joystick values from any source.
* joystick values from any source. The calculated values will be squared to decrease
* sensitivity at low speeds.
*
* @param moveValue The value to use for forwards/backwards
* @param rotateValue The value to use for the rotate right/left
@@ -486,9 +493,9 @@ public class RobotDrive implements MotorSafety {
* so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
* top, the roller axles should form an X across the robot.
*
* @param magnitude The speed that the robot should drive in a given direction.
* @param direction The direction the robot should drive in degrees. The direction and magnitude
* are independent of the rotation rate.
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The angle the robot should drive in degrees. The direction and magnitude
* are independent of the rotation rate. [-180.0..180.0]
* @param rotation The rate of rotation for the robot that is completely independent of the
* magnitude or direction. [-1.0..1.0]
*/
@@ -501,7 +508,7 @@ public class RobotDrive implements MotorSafety {
// Normalized for full power along the Cartesian axes.
magnitude = limit(magnitude) * Math.sqrt(2.0);
// The rollers are at 45 degree angles.
double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
double dirInRad = (direction + 45.0) * Math.PI / 180.0;
double cosD = Math.cos(dirInRad);
double sinD = Math.sin(dirInRad);
@@ -529,7 +536,7 @@ public class RobotDrive implements MotorSafety {
* <p>This is an alias to mecanumDrive_Polar() for backward compatibility
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The direction the robot should drive. The direction and maginitute are
* @param direction The direction the robot should drive. The direction and maginitude are
* independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely independent of the
* magnitute or direction. [-1.0..1.0]
@@ -601,8 +608,8 @@ public class RobotDrive implements MotorSafety {
*/
@SuppressWarnings("ParameterName")
protected static double[] rotateVector(double x, double y, double angle) {
double cosA = Math.cos(angle * (3.14159 / 180.0));
double sinA = Math.sin(angle * (3.14159 / 180.0));
double cosA = Math.cos(angle * (Math.PI / 180.0));
double sinA = Math.sin(angle * (Math.PI / 180.0));
double[] out = new double[2];
out[0] = x * cosA - y * sinA;
out[1] = x * sinA + y * cosA;

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
View File

@@ -18,7 +18,8 @@ import edu.wpi.first.wpilibj.hal.HAL;
*
* <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
* (e.g., 6WD or 8WD). This class takes a SpeedController per side. For four and
* six motor drivetrains, construct and pass in {@link SpeedControllerGroup} instances as follows.
* six motor drivetrains, construct and pass in {@link edu.wpi.first.wpilibj.SpeedControllerGroup}
* instances as follows.
*
* <p>Four motor drivetrain:
* <pre><code>
@@ -73,6 +74,20 @@ import edu.wpi.first.wpilibj.hal.HAL;
* <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
* points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
* positive.
*
* <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
* be set to 0, and larger values will be scaled so that the full range is still used. This
* deadband value can be changed with {@link #setDeadband}.
*
* <p>RobotDrive porting guide:
* <br>{@link #tankDrive(double, double)} is equivalent to
* {@link edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used.
* <br>{@link #arcadeDrive(double, double)} is equivalent to
* {@link edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used
* and the the rotation input is inverted eg arcadeDrive(y, -rotation)
* <br>{@link #curvatureDrive(double, double, boolean)} is similar in concept to
* {@link edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn
* mode. However, it is not designed to give exactly the same response.
*/
public class DifferentialDrive extends RobotDriveBase {
public static final double kDefaultQuickStopThreshold = 0.2;
@@ -99,6 +114,7 @@ public class DifferentialDrive extends RobotDriveBase {
/**
* Arcade drive method for differential drive platform.
* The calculated values will be squared to decrease sensitivity at low speeds.
*
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
@@ -246,6 +262,7 @@ public class DifferentialDrive extends RobotDriveBase {
/**
* Tank drive method for differential drive platform.
* The calculated values will be squared to decrease sensitivity at low speeds.
*
* @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is
* positive.

15
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
View File

@@ -39,6 +39,21 @@ import edu.wpi.first.wpilibj.hal.HAL;
* <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
* points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
* positive.
*
* <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
* be set to 0, and larger values will be scaled so that the full range is still used. This
* deadband value can be changed with {@link #setDeadband}.
*
* <p>RobotDrive porting guide:
* <br>In MecanumDrive, the right side speed controllers are automatically inverted, while in
* RobotDrive, no speed controllers are automatically inverted.
* <br>{@link #driveCartesian(double, double, double, double)} is equivalent to
* {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Cartesian(double, double, double, double)}
* if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted compared to
* RobotDrive (eg driveCartesian(xSpeed, -ySpeed, zRotation, -gyroAngle).
* <br>{@link #drivePolar(double, double, double)} is equivalent to
* {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Polar(double, double, double)} if a
* deadband of 0 is used.
*/
public class MecanumDrive extends RobotDriveBase {
private SpeedController m_frontLeftMotor;

20
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
View File

@@ -14,8 +14,11 @@ import edu.wpi.first.wpilibj.MotorSafetyHelper;
* Common base class for drive platforms.
*/
public abstract class RobotDriveBase implements MotorSafety {
protected double m_deadband = 0.02;
protected double m_maxOutput = 1.0;
public static final double kDefaultDeadband = 0.02;
public static final double kDefaultMaxOutput = 1.0;
protected double m_deadband = kDefaultDeadband;
protected double m_maxOutput = kDefaultMaxOutput;
protected MotorSafetyHelper m_safetyHelper = new MotorSafetyHelper(this);
/**
@@ -37,13 +40,22 @@ public abstract class RobotDriveBase implements MotorSafety {
m_safetyHelper.setSafetyEnabled(true);
}
/**
* Change the default value for deadband scaling. The default value is
* {@value #kDefaultDeadband}. Values smaller then the deadband are set to 0, while values
* larger then the deadband are scaled from 0.0 to 1.0. See {@link #applyDeadband}.
*
* @param deadband The deadband to set.
*/
public void setDeadband(double deadband) {
m_deadband = deadband;
}
/**
* Configure the scaling factor for using RobotDrive with motor controllers in a mode other than
* PercentVbus.
* Configure the scaling factor for using drive methods with motor controllers in a mode other
* than PercentVbus or to limit the maximum output.
*
* <p>The default value is {@value #kDefaultMaxOutput}.
*
* @param maxOutput Multiplied with the output percentage computed by the drive functions.
*/

183
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RobotDriveTest.java Normal file
View File

@@ -0,0 +1,183 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-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. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
import org.junit.BeforeClass;
import org.junit.Test;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.test.AbstractComsSetup;
import java.util.logging.Logger;
import static org.junit.Assert.assertEquals;
/**
* Tests the eqivilance of RobotDrive and DifferentialDrive/MecanumDrive.
*/
public class RobotDriveTest extends AbstractComsSetup {
private static final Logger logger = Logger.getLogger(RobotDriveTest.class.getName());
private static MockSpeedController m_rdFrontLeft = new MockSpeedController();
private static MockSpeedController m_rdRearLeft = new MockSpeedController();
private static MockSpeedController m_rdFrontRight = new MockSpeedController();
private static MockSpeedController m_rdRearRight = new MockSpeedController();
private static MockSpeedController m_frontLeft = new MockSpeedController();
private static MockSpeedController m_rearLeft = new MockSpeedController();
private static MockSpeedController m_frontRight = new MockSpeedController();
private static MockSpeedController m_rearRight = new MockSpeedController();
private static RobotDrive m_robotDrive =
new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight);
private static DifferentialDrive m_differentialDrive =
new DifferentialDrive(m_frontLeft, m_frontRight);
private static MecanumDrive m_mecanumDrive =
new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9,
-1.0};
private final double[] m_testGyroValues = {0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540,
-45, -90, -135, -180, -225, -270, -305, -360, -540};
@BeforeClass
public static void before() {
m_differentialDrive.setDeadband(0.0);
m_differentialDrive.setSafetyEnabled(false);
m_mecanumDrive.setDeadband(0.0);
m_mecanumDrive.setSafetyEnabled(false);
m_robotDrive.setSafetyEnabled(false);
}
@Test
public void testTankDriveSquared() {
for (double leftJoystick : m_testJoystickValues) {
for (double rightJoystick : m_testJoystickValues) {
m_robotDrive.tankDrive(leftJoystick, rightJoystick);
m_differentialDrive.tankDrive(leftJoystick, rightJoystick);
assertEquals("Left Motor squared didn't match. Left Joystick: " + leftJoystick
+ " Right Joystick: " + rightJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(),
0.01);
assertEquals("Right Motor squared didn't match. Left Joystick: " + leftJoystick
+ " Right Joystick: " + rightJoystick + " ", m_rdFrontRight.get(), m_frontRight.get(),
0.01);
}
}
}
@Test
public void testTankDrive() {
for (double leftJoystick : m_testJoystickValues) {
for (double rightJoystick : m_testJoystickValues) {
m_robotDrive.tankDrive(leftJoystick, rightJoystick, false);
m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false);
assertEquals("Left Motor didn't match. Left Joystick: " + leftJoystick
+ " Right Joystick: " + rightJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(),
0.01);
assertEquals("Right Motor didn't match. Left Joystick: " + leftJoystick
+ " Right Joystick: " + rightJoystick + " ", m_rdFrontRight.get(), m_frontRight.get(),
0.01);
}
}
}
@Test
public void testArcadeDriveSquared() {
for (double moveJoystick : m_testJoystickValues) {
for (double rotateJoystick : m_testJoystickValues) {
m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick);
m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick);
assertEquals("Left Motor squared didn't match. Move Joystick: " + moveJoystick
+ " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(),
0.01);
assertEquals("Right Motor squared didn't match. Move Joystick: " + moveJoystick
+ " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontRight.get(),
m_frontRight.get(), 0.01);
}
}
}
@Test
public void testArcadeDrive() {
for (double moveJoystick : m_testJoystickValues) {
for (double rotateJoystick : m_testJoystickValues) {
m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false);
m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false);
assertEquals("Left Motor didn't match. Move Joystick: " + moveJoystick
+ " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(),
0.01);
assertEquals("Right Motor didn't match. Move Joystick: " + moveJoystick
+ " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontRight.get(),
m_frontRight.get(), 0.01);
}
}
}
@Test
public void testMecanumPolar() {
System.out.println("magnitudeJoystick, directionJoystick , rotationJoystick, "
+ "m_rdFrontLeft, m_frontLeft, m_rdFrontRight, m_frontRight, m_rdRearLeft, "
+ "m_rearLeft, m_rdRearRight, m_rearRight");
for (double magnitudeJoystick : m_testJoystickValues) {
for (double directionJoystick : m_testGyroValues) {
for (double rotationJoystick : m_testJoystickValues) {
m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick);
m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick);
assertEquals("Left Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ rotationJoystick, m_rdFrontLeft.get(), m_frontLeft.get(), 0.01);
assertEquals("Right Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ rotationJoystick, m_rdFrontRight.get(), -m_frontRight.get(), 0.01);
assertEquals("Left Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ rotationJoystick, m_rdRearLeft.get(), m_rearLeft.get(), 0.01);
assertEquals("Right Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ rotationJoystick, m_rdRearRight.get(), -m_rearRight.get(), 0.01);
}
}
}
}
@Test
@SuppressWarnings("checkstyle:LocalVariableName")
public void testMecanumCartesian() {
for (double x_Joystick : m_testJoystickValues) {
for (double y_Joystick : m_testJoystickValues) {
for (double rotationJoystick : m_testJoystickValues) {
for (double gyroValue : m_testGyroValues) {
m_robotDrive.mecanumDrive_Cartesian(x_Joystick, y_Joystick, rotationJoystick,
gyroValue);
m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue);
assertEquals("Left Front Motor didn't match. X Joystick: " + x_Joystick
+ " Y Joystick: " + y_Joystick + " RotationJoystick: "
+ rotationJoystick + " Gyro: " + gyroValue, m_rdFrontLeft.get(),
m_frontLeft.get(), 0.01);
assertEquals("Right Front Motor didn't match. X Joystick: " + x_Joystick
+ " Y Joystick: " + y_Joystick + " RotationJoystick: "
+ rotationJoystick + " Gyro: " + gyroValue, m_rdFrontRight.get(),
-m_frontRight.get(), 0.01);
assertEquals("Left Rear Motor didn't match. X Joystick: " + x_Joystick
+ " Y Joystick: " + y_Joystick + " RotationJoystick: "
+ rotationJoystick + " Gyro: " + gyroValue, m_rdRearLeft.get(),
m_rearLeft.get(), 0.01);
assertEquals("Right Rear Motor didn't match. X Joystick: " + x_Joystick
+ " Y Joystick: " + y_Joystick + " RotationJoystick: "
+ rotationJoystick + " Gyro: " + gyroValue, m_rdRearRight.get(),
-m_rearRight.get(), 0.01);
}
}
}
}
}
protected Logger getClassLogger() {
return logger;
}
}

2
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java
View File

@@ -25,6 +25,6 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite;
EncoderTest.class, FilterNoiseTest.class, FilterOutputTest.class, GyroTest.class,
MotorEncoderTest.class, MotorInvertingTest.class, PCMTest.class, PDPTest.class,
PIDTest.class, PIDToleranceTest.class, PreferencesTest.class, RelayCrossConnectTest.class,
SampleTest.class, TimerTest.class})
RobotDriveTest.class, SampleTest.class, TimerTest.class})
public class WpiLibJTestSuite extends AbstractTestSuite {
}