Skip to content

Commit 481b640

Browse files
virtualdvirtuald
committed
Add translation of MAXSwerve-Java-Template
1 parent 90f7e32 commit 481b640

File tree

6 files changed

+811
-0
lines changed

6 files changed

+811
-0
lines changed

‎examples/maxswerve/constants.py‎

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
# Copyright (c) FIRST and other WPILib contributors.
2+
# Open Source Software; you can modify and/or share it under the terms of
3+
# the WPILib BSD license file in the root directory of this project.
4+
5+
"""
6+
The constants module is a convenience place for teams to hold robot-wide
7+
numerical or boolean constants. Don't use this for any other purpose!
8+
"""
9+
10+
11+
import math
12+
13+
from wpimath import units
14+
from wpimath.geometry import Translation2d
15+
from wpimath.kinematics import SwerveDrive4Kinematics
16+
from wpimath.trajectory import TrapezoidProfileRadians
17+
18+
from rev import CANSparkMax
19+
20+
21+
class NeoMotorConstants:
22+
kFreeSpeedRpm = 5676
23+
24+
25+
class DriveConstants:
26+
# Driving Parameters - Note that these are not the maximum capable speeds of
27+
# the robot, rather the allowed maximum speeds
28+
kMaxSpeedMetersPerSecond = 4.8
29+
kMaxAngularSpeed = 2 * math.pi # radians per second
30+
31+
kDirectionSlewRate = 1.2 # radians per second
32+
kMagnitudeSlewRate = 1.8 # percent per second (1 = 100%)
33+
kRotationalSlewRate = 2.0 # percent per second (1 = 100%)
34+
35+
# Chassis configuration
36+
kTrackWidth = units.inchesToMeters(26.5)
37+
# Distance between centers of right and left wheels on robot
38+
kWheelBase = units.inchesToMeters(26.5)
39+
40+
# Distance between front and back wheels on robot
41+
kModulePositions = [
42+
Translation2d(kWheelBase / 2, kTrackWidth / 2),
43+
Translation2d(kWheelBase / 2, -kTrackWidth / 2),
44+
Translation2d(-kWheelBase / 2, kTrackWidth / 2),
45+
Translation2d(-kWheelBase / 2, -kTrackWidth / 2),
46+
]
47+
kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions)
48+
49+
# Angular offsets of the modules relative to the chassis in radians
50+
kFrontLeftChassisAngularOffset = -math.pi / 2
51+
kFrontRightChassisAngularOffset = 0
52+
kBackLeftChassisAngularOffset = math.pi
53+
kBackRightChassisAngularOffset = math.pi / 2
54+
55+
# SPARK MAX CAN IDs
56+
kFrontLeftDrivingCanId = 11
57+
kRearLeftDrivingCanId = 13
58+
kFrontRightDrivingCanId = 15
59+
kRearRightDrivingCanId = 17
60+
61+
kFrontLeftTurningCanId = 10
62+
kRearLeftTurningCanId = 12
63+
kFrontRightTurningCanId = 14
64+
kRearRightTurningCanId = 16
65+
66+
kGyroReversed = False
67+
68+
69+
class ModuleConstants:
70+
# The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T.
71+
# This changes the drive speed of the module (a pinion gear with more teeth will result in a
72+
# robot that drives faster).
73+
kDrivingMotorPinionTeeth = 14
74+
75+
# Invert the turning encoder, since the output shaft rotates in the opposite direction of
76+
# the steering motor in the MAXSwerve Module.
77+
kTurningEncoderInverted = True
78+
79+
# Calculations required for driving motor conversion factors and feed forward
80+
kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60
81+
kWheelDiameterMeters = 0.0762
82+
kWheelCircumferenceMeters = kWheelDiameterMeters * math.pi
83+
# 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
84+
kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15)
85+
kDriveWheelFreeSpeedRps = (
86+
kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters
87+
) / kDrivingMotorReduction
88+
89+
kDrivingEncoderPositionFactor = (
90+
kWheelDiameterMeters * math.pi
91+
) / kDrivingMotorReduction # meters
92+
kDrivingEncoderVelocityFactor = (
93+
(kWheelDiameterMeters * math.pi) / kDrivingMotorReduction
94+
) / 60.0 # meters per second
95+
96+
kTurningEncoderPositionFactor = 2 * math.pi # radian
97+
kTurningEncoderVelocityFactor = (2 * math.pi) / 60.0 # radians per second
98+
99+
kTurningEncoderPositionPIDMinInput = 0 # radian
100+
kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor # radian
101+
102+
kDrivingP = 0.04
103+
kDrivingI = 0
104+
kDrivingD = 0
105+
kDrivingFF = 1 / kDriveWheelFreeSpeedRps
106+
kDrivingMinOutput = -1
107+
kDrivingMaxOutput = 1
108+
109+
kTurningP = 1
110+
kTurningI = 0
111+
kTurningD = 0
112+
kTurningFF = 0
113+
kTurningMinOutput = -1
114+
kTurningMaxOutput = 1
115+
116+
kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake
117+
kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake
118+
119+
kDrivingMotorCurrentLimit = 50 # amp
120+
kTurningMotorCurrentLimit = 20 # amp
121+
122+
123+
class OIConstants:
124+
kDriverControllerPort = 0
125+
kDriveDeadband = 0.05
126+
127+
128+
class AutoConstants:
129+
kMaxSpeedMetersPerSecond = 3
130+
kMaxAccelerationMetersPerSecondSquared = 3
131+
kMaxAngularSpeedRadiansPerSecond = math.pi
132+
kMaxAngularSpeedRadiansPerSecondSquared = math.pi
133+
134+
kPXController = 1
135+
kPYController = 1
136+
kPThetaController = 1
137+
138+
# Constraint for the motion profiled robot angle controller
139+
kThetaControllerConstraints = TrapezoidProfileRadians.Constraints(
140+
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared
141+
)

‎examples/maxswerve/robot.py‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) FIRST and other WPILib contributors.
4+
# Open Source Software; you can modify and/or share it under the terms of
5+
# the WPILib BSD license file in the root directory of this project.
6+
#
7+
8+
import commands2
9+
import wpilib
10+
11+
from robotcontainer import RobotContainer
12+
13+
14+
class MyRobot(commands2.TimedCommandRobot):
15+
def robotInit(self):
16+
# Instantiate our RobotContainer. This will perform all our button bindings, and put our
17+
# autonomous chooser on the dashboard.
18+
self.container = RobotContainer()
19+
self.autonomousCommand = None
20+
21+
def autonomousInit(self) -> None:
22+
self.autonomousCommand = self.container.getAutonomousCommand()
23+
24+
if self.autonomousCommand:
25+
self.autonomousCommand.schedule()
26+
27+
def teleopInit(self) -> None:
28+
if self.autonomousCommand:
29+
self.autonomousCommand.cancel()
30+
31+
def testInit(self) -> None:
32+
commands2.CommandScheduler.getInstance().cancelAll()
33+
34+
35+
if __name__ == "__main__":
36+
wpilib.run(MyRobot)

‎examples/maxswerve/robotcontainer.py‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
import math
2+
3+
import commands2
4+
import wpimath
5+
import wpilib
6+
7+
from wpimath.controller import PIDController, ProfiledPIDControllerRadians
8+
from wpimath.geometry import Pose2d, Rotation2d, Translation2d
9+
from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
10+
11+
from constants import AutoConstants, DriveConstants, OIConstants
12+
from subsystems.drivesubsystem import DriveSubsystem
13+
14+
15+
class RobotContainer:
16+
"""
17+
This class is where the bulk of the robot should be declared. Since Command-based is a
18+
"declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
19+
periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
20+
subsystems, commands, and button mappings) should be declared here.
21+
"""
22+
23+
def __init__(self) -> None:
24+
# The robot's subsystems
25+
self.robotDrive = DriveSubsystem()
26+
27+
# The driver's controller
28+
self.driverController = wpilib.XboxController(OIConstants.kDriverControllerPort)
29+
30+
# Configure the button bindings
31+
self.configureButtonBindings()
32+
33+
# Configure default commands
34+
self.robotDrive.setDefaultCommand(
35+
# The left stick controls translation of the robot.
36+
# Turning is controlled by the X axis of the right stick.
37+
commands2.RunCommand(
38+
lambda: self.robotDrive.drive(
39+
-wpimath.applyDeadband(
40+
self.driverController.getLeftY(), OIConstants.kDriveDeadband
41+
),
42+
-wpimath.applyDeadband(
43+
self.driverController.getLeftX(), OIConstants.kDriveDeadband
44+
),
45+
-wpimath.applyDeadband(
46+
self.driverController.getRightX(), OIConstants.kDriveDeadband
47+
),
48+
True,
49+
True,
50+
),
51+
[self.robotDrive],
52+
)
53+
)
54+
55+
def configureButtonBindings(self) -> None:
56+
"""
57+
Use this method to define your button->command mappings. Buttons can be created by
58+
instantiating a :GenericHID or one of its subclasses (Joystick or XboxController),
59+
and then passing it to a JoystickButton.
60+
"""
61+
62+
def disablePIDSubsystems(self) -> None:
63+
"""Disables all ProfiledPIDSubsystem and PIDSubsystem instances.
64+
This should be called on robot disable to prevent integral windup."""
65+
66+
def getAutonomousCommand(self) -> commands2.Command:
67+
"""Use this to pass the autonomous command to the main {@link Robot} class.
68+
69+
:returns: the command to run in autonomous
70+
"""
71+
# Create config for trajectory
72+
config = TrajectoryConfig(
73+
AutoConstants.kMaxSpeedMetersPerSecond,
74+
AutoConstants.kMaxAccelerationMetersPerSecondSquared,
75+
)
76+
# Add kinematics to ensure max speed is actually obeyed
77+
config.setKinematics(DriveConstants.kDriveKinematics)
78+
79+
# An example trajectory to follow. All units in meters.
80+
exampleTrajectory = TrajectoryGenerator.generateTrajectory(
81+
# Start at the origin facing the +X direction
82+
Pose2d(0, 0, Rotation2d(0)),
83+
# Pass through these two interior waypoints, making an 's' curve path
84+
[Translation2d(1, 1), Translation2d(2, -1)],
85+
# End 3 meters straight ahead of where we started, facing forward
86+
Pose2d(3, 0, Rotation2d(0)),
87+
config,
88+
)
89+
90+
thetaController = ProfiledPIDControllerRadians(
91+
AutoConstants.kPThetaController,
92+
0,
93+
0,
94+
AutoConstants.kThetaControllerConstraints,
95+
)
96+
thetaController.enableContinuousInput(-math.pi, math.pi)
97+
98+
swerveControllerCommand = commands2.Swerve4ControllerCommand(
99+
exampleTrajectory,
100+
self.robotDrive.getPose, # Functional interface to feed supplier
101+
DriveConstants.kDriveKinematics,
102+
# Position controllers
103+
PIDController(AutoConstants.kPXController, 0, 0),
104+
PIDController(AutoConstants.kPYController, 0, 0),
105+
thetaController,
106+
self.robotDrive.setModuleStates,
107+
[self.robotDrive],
108+
)
109+
110+
# Reset odometry to the starting pose of the trajectory.
111+
self.robotDrive.resetOdometry(exampleTrajectory.initialPose())
112+
113+
# Run path following command, then stop at the end.
114+
return swerveControllerCommand.andThen(
115+
lambda: self.robotDrive.drive(0, 0, 0, False, False)
116+
)

0 commit comments

Comments
 (0)