forked from robotpy/pyfrc
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtest_drivetrains.py
112 lines (103 loc) · 3.77 KB
/
test_drivetrains.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
import pytest
from pyfrc.physics import drivetrains
from pyfrc.physics.units import units
from math import sqrt
@pytest.mark.parametrize(
"l_motor,r_motor,output",
[
(0, 0, (0, 0)), # stationary
(1, -1, (1, 0)), # forward
(-1, 1, (-1, 0)), # backwards
(-1, -1, (0, 1)), # rotate left
(1, 1, (0, -1)), # rotate right
],
)
def test_two_motor_drivetrain(l_motor, r_motor, output):
result = drivetrains.TwoMotorDrivetrain(speed=1 * units.fps).calculate(
l_motor, r_motor
)
result = (result.vx_fps, result.omega)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 0.001
@pytest.mark.parametrize(
"lf_motor,lr_motor,rf_motor,rr_motor,output",
[
(0, 0, 0, 0, (0, 0)), # stationary
(1, 1, -1, -1, (1, 0)), # forward
(-1, -1, 1, 1, (-1, 0)), # backwards
(-1, -1, -1, -1, (0, 1)), # rotate left
(1, 1, 1, 1, (0, -1)), # rotate right
],
)
def test_four_motor_drivetrain(lf_motor, lr_motor, rf_motor, rr_motor, output):
result = drivetrains.FourMotorDrivetrain(speed=1 * units.fps).calculate(
lf_motor, lr_motor, rf_motor, rr_motor
)
result = (result.vx_fps, result.omega)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 0.001
# @pytest.mark.parametrize(
# "lr_motor,rr_motor,lf_motor,rf_motor,output",
# [
# (0, 0, 0, 0, (0, 0, 0)), # stationary
# (1, 1, 1, 1, (0, 1, 0)), # forward
# (-1, -1, -1, -1, (0, -1, 0)), # backwards
# (1, -1, -1, 1, (-1, 0, 0)), # strafe left
# (-1, 1, 1, -1, (1, 0, 0)), # strafe right
# (-1, 1, -1, 1, (0, 0, 1)), # rotate left
# (1, -1, 1, -1, (0, 0, -1)), # rotate right
# ],
# )
# def test_mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, output):
# result = drivetrains.MecanumDrivetrain(
# x_wheelbase=1, y_wheelbase=1, speed=1,
# ).get_vector(lf_motor, lr_motor, rf_motor, rr_motor)
# assert abs(result[0] - output[0]) < 0.001
# assert abs(result[1] - output[1]) < 0.001
# assert abs(result[2] - output[2]) < 0.001
@pytest.mark.parametrize(
"lr_motor,rr_motor,lf_motor,rf_motor,lr_angle,rr_angle,lf_angle,rf_angle,output",
[
(0, 0, 0, 0, 0, 0, 0, 0, (0, 0, 0)), # stationary
(1, 1, 1, 1, 0, 0, 0, 0, (0, 1, 0)), # forward
(-1, -1, -1, -1, 180, 180, 180, 180, (0, 1, 0)), # forward inverted
(-1, -1, -1, -1, 0, 0, 0, 0, (0, -1, 0)), # backward
(1, 1, 1, 1, 180, 180, 180, 180, (0, -1, 0)), # backward inverted
(1, 1, 1, 1, 270, 270, 270, 270, (-1, 0, 0)), # strafe left
(-1, -1, -1, -1, 90, 90, 90, 90, (-1, 0, 0)), # strafe left inverted
(1, 1, 1, 1, 90, 90, 90, 90, (1, 0, 0)), # strafe right
(-1, -1, -1, -1, 270, 270, 270, 270, (1, 0, 0)), # strafe right inverted
(1, 1, 1, 1, 135, 45, 225, 315, (0, 0, 1)), # rotate left
(-1, -1, -1, -1, 315, 225, 45, 135, (0, 0, 1)), # rotate left inverted
(1, 1, 1, 1, 315, 225, 45, 135, (0, 0, -1)), # rotate right
(-1, -1, -1, -1, 135, 45, 225, 315, (0, 0, -1)), # rotate right inverted
],
)
def test_four_motor_swerve_drivetrain(
lr_motor,
rr_motor,
lf_motor,
rf_motor,
lr_angle,
rr_angle,
lf_angle,
rf_angle,
output,
):
wheelbase = sqrt(2)
result = drivetrains.four_motor_swerve_drivetrain(
lr_motor,
rr_motor,
lf_motor,
rf_motor,
lr_angle,
rr_angle,
lf_angle,
rf_angle,
speed=1,
x_wheelbase=wheelbase,
y_wheelbase=wheelbase,
)
assert abs(result.vx_fps - output[0]) < 0.001
assert abs(result.vy_fps - output[1]) < 0.001
assert abs(result.omega - output[2]) < 0.001