Repository navigation
Expand file tree
/
Copy pathDriveTrainSubsystem.py
More file actions
103 lines (91 loc) · 3.56 KB
/
Copy pathDriveTrainSubsystem.py
File metadata and controls
103 lines (91 loc) · 3.56 KB
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
#
# 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.
#
import math
import wpilib
import wpimath.geometry
import wpimath.kinematics
import swervemodule
from phoenix6.hardware import Pigeon2
import constants
kMaxSpeed = 3.0 # 3 meters per second
kMaxAngularSpeed = math.pi # 1/2 rotation per second
class DriveTrain:
"""
Represents a swerve drive style drivetrain.
"""
def __init__(self) -> None:
self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3)
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7)
self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11)
self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15)
self.gyro = Pigeon2(0)
self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
self.frontLeftLocation,
self.frontRightLocation,
self.backLeftLocation,
self.backRightLocation,
)
self.odometry = wpimath.kinematics.SwerveDrive4Odometry(
self.kinematics,
self.gyro.getRotation2d(),
(
self.frontLeft.getPosition(),
self.frontRight.getPosition(),
self.backLeft.getPosition(),
self.backRight.getPosition(),
),
)
self.gyro.reset()
def drive(
self,
xSpeed: float,
ySpeed: float,
rot: float,
fieldRelative: bool,
periodSeconds: float,
) -> None:
"""
Method to drive the robot using joystick info.
:param xSpeed: Speed of the robot in the x direction (forward).
:param ySpeed: Speed of the robot in the y direction (sideways).
:param rot: Angular rate of the robot.
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
:param periodSeconds: Time
"""
swerveModuleStates = self.kinematics.toSwerveModuleStates(
wpimath.kinematics.ChassisSpeeds.discretize(
(
wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, self.gyro.getRotation2d()
)
if fieldRelative
else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
),
periodSeconds,
)
)
wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
swerveModuleStates, kMaxSpeed
)
self.frontLeft.setDesiredState(swerveModuleStates[0])
self.frontRight.setDesiredState(swerveModuleStates[1])
self.backLeft.setDesiredState(swerveModuleStates[2])
self.backRight.setDesiredState(swerveModuleStates[3])
def updateOdometry(self) -> None:
"""Updates the field relative position of the robot."""
self.odometry.update(
self.gyro.getRotation2d(),
(
self.frontLeft.getPosition(),
self.frontRight.getPosition(),
self.backLeft.getPosition(),
self.backRight.getPosition(),
),
)