Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / aos / 60246090c818a75ac60fad4ec4d540d8ca428157 / . / wpilibc / src / test / native / cpp / RobotDriveTest.cpp
blob: 464a7074fdf1e49842ec4e343210c18624167914 [file] [log] [blame]
Brian Silverman41cdd3e2019-01-19 19:48:58 -0800[diff] [blame]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
3/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#include "MockSpeedController.h"
9#include "frc/RobotDrive.h"
10#include "frc/drive/DifferentialDrive.h"
11#include "frc/drive/MecanumDrive.h"
12#include "gtest/gtest.h"
13
14using namespace frc;
15
16class RobotDriveTest : public testing::Test {
17 protected:
18 MockSpeedController m_rdFrontLeft;
19 MockSpeedController m_rdRearLeft;
20 MockSpeedController m_rdFrontRight;
21 MockSpeedController m_rdRearRight;
22 MockSpeedController m_frontLeft;
23 MockSpeedController m_rearLeft;
24 MockSpeedController m_frontRight;
25 MockSpeedController m_rearRight;
26 frc::RobotDrive m_robotDrive{m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight,
27 m_rdRearRight};
28 frc::DifferentialDrive m_differentialDrive{m_frontLeft, m_frontRight};
29 frc::MecanumDrive m_mecanumDrive{m_frontLeft, m_rearLeft, m_frontRight,
30 m_rearRight};
31
32 double m_testJoystickValues[9] = {-1.0, -0.9, -0.5, -0.01, 0.0,
33 0.01, 0.5, 0.9, 1.0};
34 double m_testGyroValues[19] = {0, 45, 90, 135, 180, 225, 270,
35 305, 360, 540, -45, -90, -135, -180,
36 -225, -270, -305, -360, -540};
37};
38
39TEST_F(RobotDriveTest, TankDrive) {
40 int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
41 double leftJoystick, rightJoystick;
42 m_differentialDrive.SetDeadband(0.0);
43 m_differentialDrive.SetSafetyEnabled(false);
44 m_mecanumDrive.SetSafetyEnabled(false);
45 m_robotDrive.SetSafetyEnabled(false);
46 for (int i = 0; i < joystickSize; i++) {
47 for (int j = 0; j < joystickSize; j++) {
48 leftJoystick = m_testJoystickValues[i];
49 rightJoystick = m_testJoystickValues[j];
50 m_robotDrive.TankDrive(leftJoystick, rightJoystick, false);
51 m_differentialDrive.TankDrive(leftJoystick, rightJoystick, false);
52 ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
53 ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
54 }
55 }
56}
57
58TEST_F(RobotDriveTest, TankDriveSquared) {
59 int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
60 double leftJoystick, rightJoystick;
61 m_differentialDrive.SetDeadband(0.0);
62 m_differentialDrive.SetSafetyEnabled(false);
63 m_mecanumDrive.SetSafetyEnabled(false);
64 m_robotDrive.SetSafetyEnabled(false);
65 for (int i = 0; i < joystickSize; i++) {
66 for (int j = 0; j < joystickSize; j++) {
67 leftJoystick = m_testJoystickValues[i];
68 rightJoystick = m_testJoystickValues[j];
69 m_robotDrive.TankDrive(leftJoystick, rightJoystick, true);
70 m_differentialDrive.TankDrive(leftJoystick, rightJoystick, true);
71 ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
72 ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
73 }
74 }
75}
76
77TEST_F(RobotDriveTest, ArcadeDriveSquared) {
78 int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
79 double moveJoystick, rotateJoystick;
80 m_differentialDrive.SetDeadband(0.0);
81 m_differentialDrive.SetSafetyEnabled(false);
82 m_mecanumDrive.SetSafetyEnabled(false);
83 m_robotDrive.SetSafetyEnabled(false);
84 for (int i = 0; i < joystickSize; i++) {
85 for (int j = 0; j < joystickSize; j++) {
86 moveJoystick = m_testJoystickValues[i];
87 rotateJoystick = m_testJoystickValues[j];
88 m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, true);
89 m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, true);
90 ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
91 ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
92 }
93 }
94}
95
96TEST_F(RobotDriveTest, ArcadeDrive) {
97 int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
98 double moveJoystick, rotateJoystick;
99 m_differentialDrive.SetDeadband(0.0);
100 m_differentialDrive.SetSafetyEnabled(false);
101 m_mecanumDrive.SetSafetyEnabled(false);
102 m_robotDrive.SetSafetyEnabled(false);
103 for (int i = 0; i < joystickSize; i++) {
104 for (int j = 0; j < joystickSize; j++) {
105 moveJoystick = m_testJoystickValues[i];
106 rotateJoystick = m_testJoystickValues[j];
107 m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, false);
108 m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, false);
109 ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
110 ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
111 }
112 }
113}
114
115TEST_F(RobotDriveTest, MecanumCartesian) {
116 int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
117 int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
118 double xJoystick, yJoystick, rotateJoystick, gyroValue;
119 m_mecanumDrive.SetDeadband(0.0);
120 m_mecanumDrive.SetSafetyEnabled(false);
121 m_differentialDrive.SetSafetyEnabled(false);
122 m_robotDrive.SetSafetyEnabled(false);
123 for (int i = 0; i < joystickSize; i++) {
124 for (int j = 0; j < joystickSize; j++) {
125 for (int k = 0; k < joystickSize; k++) {
126 for (int l = 0; l < gyroSize; l++) {
127 xJoystick = m_testJoystickValues[i];
128 yJoystick = m_testJoystickValues[j];
129 rotateJoystick = m_testJoystickValues[k];
130 gyroValue = m_testGyroValues[l];
131 m_robotDrive.MecanumDrive_Cartesian(xJoystick, yJoystick,
132 rotateJoystick, gyroValue);
133 m_mecanumDrive.DriveCartesian(xJoystick, -yJoystick, rotateJoystick,
134 -gyroValue);
135 ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
136 << "X: " << xJoystick << " Y: " << yJoystick
137 << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
138 ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
139 << "X: " << xJoystick << " Y: " << yJoystick
140 << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
141 ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
142 << "X: " << xJoystick << " Y: " << yJoystick
143 << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
144 ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
145 << "X: " << xJoystick << " Y: " << yJoystick
146 << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
147 }
148 }
149 }
150 }
151}
152
153TEST_F(RobotDriveTest, MecanumPolar) {
154 int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
155 int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
156 double magnitudeJoystick, directionJoystick, rotateJoystick;
157 m_mecanumDrive.SetDeadband(0.0);
158 m_mecanumDrive.SetSafetyEnabled(false);
159 m_differentialDrive.SetSafetyEnabled(false);
160 m_robotDrive.SetSafetyEnabled(false);
161 for (int i = 0; i < joystickSize; i++) {
162 for (int j = 0; j < gyroSize; j++) {
163 for (int k = 0; k < joystickSize; k++) {
164 magnitudeJoystick = m_testJoystickValues[i];
165 directionJoystick = m_testGyroValues[j];
166 rotateJoystick = m_testJoystickValues[k];
167 m_robotDrive.MecanumDrive_Polar(magnitudeJoystick, directionJoystick,
168 rotateJoystick);
169 m_mecanumDrive.DrivePolar(magnitudeJoystick, directionJoystick,
170 rotateJoystick);
171 ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
172 << "Magnitude: " << magnitudeJoystick
173 << " Direction: " << directionJoystick
174 << " Rotate: " << rotateJoystick;
175 ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
176 << "Magnitude: " << magnitudeJoystick
177 << " Direction: " << directionJoystick
178 << " Rotate: " << rotateJoystick;
179 ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
180 << "Magnitude: " << magnitudeJoystick
181 << " Direction: " << directionJoystick
182 << " Rotate: " << rotateJoystick;
183 ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
184 << "Magnitude: " << magnitudeJoystick
185 << " Direction: " << directionJoystick
186 << " Rotate: " << rotateJoystick;
187 }
188 }
189 }
190}