Skip to content

Commit 0af212b

Browse files
cmcg-dfcicmcg-dfci
committed
frame 2025 bot
1 parent 154a836 commit 0af212b

File tree

5 files changed

+268
-35
lines changed

5 files changed

+268
-35
lines changed

‎src/main/java/org/usfirst/frc/team69/robot/OIMap.java‎

Lines changed: 19 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,9 @@
11
package org.usfirst.frc.team69.robot;
22

3-
import org.hyperonline.hyperlib.oi.MapJoystick;
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import edu.wpi.first.wpilibj2.command.InstantCommand;
5+
import org.hyperonline.hyperlib.oi.MapController;
6+
import org.hyperonline.hyperlib.oi.WhenPressed;
47

58
/**
69
* This class is the glue that binds the controls on the physical operator interface to the commands
@@ -9,7 +12,7 @@
912
public class OIMap {
1013
/**
1114
* button layout
12-
* https://hyperonline.org/gladiator_nxt_evo_buttons.png
15+
* TODO: add button map image
1316
*/
1417
@MapController(
1518
port = 0,
@@ -22,30 +25,32 @@ public static class LeftDriver {
2225
public Command fieldCentric = new InstantCommand(Robot.driveTrain::setFieldCentric);
2326
}
2427

25-
/**
26-
* button layout
27-
* https://hyperonline.org/gladiator_nxt_evo_buttons.png
28-
*/
2928
@MapController(
3029
port = 1,
3130
role = MapController.Role.RIGHT_DRIVER,
3231
type = MapController.Type.APEM_HF45S10U)
3332
public static class RightDriver {
3433
@WhenPressed(1)
3534
public Command resetGyro = Robot.driveTrain.zeroHeadingCmd();
36-
@WhileHeld(2)
37-
public Command alignWithTargetCmd = Robot.vision.alignWithTargetCmd();
3835
}
3936

40-
@MapJoystick(
37+
/**
38+
* button layout
39+
* https://hyperonline.org/gladiator_nxt_evo_buttons.png
40+
*/
41+
@MapController(
4142
port = 2,
42-
role = MapJoystick.Role.LEFT_OPERATOR,
43-
type = MapJoystick.Type.VKB_GLADIATOR_NXT_EVO_SCE_LEFT)
43+
role = MapController.Role.LEFT_OPERATOR,
44+
type = MapController.Type.VKB_GLADIATOR_NXT_EVO_SCE_LEFT)
4445
public static class LeftOperator {}
4546

46-
@MapJoystick(
47+
/**
48+
* button layout
49+
* https://hyperonline.org/gladiator_nxt_evo_buttons.png
50+
*/
51+
@MapController(
4752
port = 3,
48-
role = MapJoystick.Role.RIGHT_OPERATOR,
49-
type = MapJoystick.Type.VKB_GLADIATOR_NXT_EVO_SCE_LEFT)
53+
role = MapController.Role.RIGHT_OPERATOR,
54+
type = MapController.Type.VKB_GLADIATOR_NXT_EVO_SCE_RIGHT)
5055
public static class RightOperator {}
5156
}

‎src/main/java/org/usfirst/frc/team69/robot/Robot.java‎

Lines changed: 133 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,45 +1,163 @@
11
package org.usfirst.frc.team69.robot;
2-
import edu.wpi.first.networktables.NetworkTableInstance;
2+
3+
import com.pathplanner.lib.auto.NamedCommands;
4+
import edu.wpi.first.wpilibj.DriverStation;
5+
import edu.wpi.first.wpilibj.Joystick;
6+
import edu.wpi.first.wpilibj.PowerDistribution;
7+
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
8+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
9+
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
310
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
4-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
511
import edu.wpi.first.wpilibj2.command.Command;
612
import org.hyperonline.hyperlib.HYPERRobot;
13+
import org.hyperonline.hyperlib.oi.BadOIMapException;
714
import org.hyperonline.hyperlib.oi.OI;
15+
import org.hyperonline.hyperlib.pref.PreferencesListener;
16+
import org.hyperonline.hyperlib.pref.PreferencesUpdater;
817
import org.usfirst.frc.team69.robot.commands.AutonomousCommands;
9-
import org.usfirst.frc.team69.robot.subsystems.DriveTrain;
10-
11-
public class Robot extends HYPERRobot implements PreferencesListener {
18+
import org.usfirst.frc.team69.robot.subsystems.swerve.SwerveSubsystem;
1219

13-
public static OI oi;
14-
public static DriveTrain driveTrain;
1520

16-
private NetworkTableInstance m_tableInstance = NetworkTableInstance.getDefault();
21+
/**
22+
* The VM is configured to automatically run this class, and to call the functions corresponding to
23+
* each mode, as described in the IterativeRobot documentation. If you change the name of this class
24+
* or the package after creating this project, you must also update the manifest file in the
25+
* resource directory.
26+
*/
27+
public class Robot extends HYPERRobot implements PreferencesListener {
28+
public static OI<Joystick, Joystick, Joystick, Joystick> oi;
29+
public static SwerveSubsystem driveTrain;
30+
public static PowerDistribution powerDistribution;
31+
private final String driverTabName = "Drive Team";
1732
private Command autonomousCommand;
1833
private SendableChooser<Command> chooser;
34+
private ShuffleboardTab driverTab;
1935

20-
21-
public Robot(){}
36+
public Robot() {
37+
m_burnFlashOnInit = false;
38+
}
2239

2340
@Override
2441
protected void initOI() {
25-
oi = new OI(OIMap.class, true);
42+
try {
43+
oi = new OI<>(OIMap.class, true);
44+
} catch (BadOIMapException e) {
45+
DriverStation.reportError(e.getMessage(), e.getStackTrace());
46+
}
2647
}
2748

2849
@Override
2950
protected void initSubsystems() {
30-
driveTrain = new DriveTrain();
51+
// disable automatically checking of Preference updates
52+
// preferences will not be updating while the robot is running during matches,
53+
// so we do not need to clog the HYPERRobot.robotPeriodic()
54+
PreferencesUpdater.setAutoCheckForUpdate(true);
55+
56+
powerDistribution = new PowerDistribution(RobotMap.Electrical.POWER_DISTRIBUTION_PORT, RobotMap.Electrical.POWER_DISTRIBUTION_TYPE);
3157

58+
driveTrain = new SwerveSubsystem();
3259
}
3360

3461
@Override
3562
protected void initHelpers() {
36-
63+
DriverStation.silenceJoystickConnectionWarning(true);
64+
driverTab = Shuffleboard.getTab(driverTabName);
65+
if (DriverStation.getMatchType() != DriverStation.MatchType.None) {
66+
Shuffleboard.selectTab(driverTabName);
67+
}
3768
}
3869

3970
@Override
4071
protected void initCommands() {
4172
oi.initCommands();
73+
4274
chooser = new AutonomousCommands().createAutoChooser();
43-
SmartDashboard.putData("Auto Chooser", chooser);
75+
driverTab.add("Auto Chooser", chooser).withPosition(0, 0).withSize(2, 1);
76+
}
77+
78+
/**
79+
* This autonomous (along with the chooser code above) shows how to select between different
80+
* autonomous modes using the dashboard. The sendable chooser code works with the Java
81+
* SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
82+
* uncomment the getString code to get the auto name from the text box below the Gyro
83+
*
84+
* <p>You can add additional auto modes by adding additional commands to the chooser code above
85+
* (like the commented example) or additional comparisons to the switch structure below with
86+
* additional strings & commands.
87+
*/
88+
@Override
89+
public void autonomousInit() {
90+
super.autonomousInit();
91+
autonomousCommand = chooser.getSelected();
92+
if (autonomousCommand != null) {
93+
autonomousCommand.schedule();
94+
}
95+
}
96+
97+
/**
98+
* This function is called periodically during autonomous
99+
*/
100+
@Override
101+
public void autonomousPeriodic() {
102+
super.autonomousPeriodic();
44103
}
45-
}
104+
105+
@Override
106+
public void teleopInit() {
107+
super.teleopInit();
108+
// This makes sure that the autonomous stops running when
109+
// teleop starts running. If you want the autonomous to
110+
// continue until interrupted by another command, remove
111+
// this line or comment it out.
112+
if (autonomousCommand != null) {
113+
autonomousCommand.cancel();
114+
}
115+
}
116+
117+
/**
118+
* This function is called periodically during operator control
119+
*/
120+
@Override
121+
public void teleopPeriodic() {
122+
super.teleopPeriodic();
123+
}
124+
125+
/**
126+
* This function is called once each time the robot enters test mode.
127+
*/
128+
@Override
129+
public void testInit() {
130+
super.testInit();
131+
LiveWindow.setEnabled(true);
132+
}
133+
134+
/**
135+
* This function is called periodically during test mode
136+
*/
137+
@Override
138+
public void testPeriodic() {
139+
super.testPeriodic();
140+
// LiveWindow.run();
141+
}
142+
143+
/**
144+
* This function is called once each time the robot enters Disabled mode. You can use it to reset
145+
* any subsystem information you want to clear when the robot is disabled.
146+
*/
147+
@Override
148+
public void disabledInit() {
149+
super.disabledInit();
150+
}
151+
152+
@Override
153+
public void disabledPeriodic() {
154+
super.disabledPeriodic();
155+
}
156+
157+
@Override
158+
public void onPreferencesUpdated() {
159+
160+
}
161+
162+
163+
}

‎src/main/java/org/usfirst/frc/team69/robot/RobotMap.java‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
package org.usfirst.frc.team69.robot;
22

3-
import edu.wpi.first.wpilibj.I2C;
3+
import edu.wpi.first.wpilibj.PowerDistribution;
44
import org.hyperonline.hyperlib.port.Port;
55
import org.hyperonline.hyperlib.port.Port.Type;
66

@@ -11,14 +11,28 @@
1111
* floating around.
1212
*/
1313
public class RobotMap {
14+
public static String camera = "hypervision";
15+
1416
public static class Electrical {
15-
@Port(type = Type.CAN) public static final int PDP = 0;
17+
// this is hardcoded for the REV board, we cannot change it
18+
@Port(type = Type.CAN) public static final int POWER_DISTRIBUTION_PORT = 1;
19+
public static final PowerDistribution.ModuleType POWER_DISTRIBUTION_TYPE = PowerDistribution.ModuleType.kRev;
1620
}
1721

1822
public static class DriveTrain {
19-
@Port(type = Type.CAN) public static final int FRONT_LEFT = 2;
20-
@Port(type = Type.CAN) public static final int FRONT_RIGHT = 3;
21-
@Port(type = Type.CAN) public static final int BACK_LEFT = 1;
22-
@Port(type = Type.CAN) public static final int BACK_RIGHT = 4;
23+
// MOTORS
24+
@Port(type = Type.CAN) public static final int FRONT_LEFT_DRIVE = 10;
25+
@Port(type = Type.CAN) public static final int FRONT_LEFT_PIVOT = 11;
26+
@Port(type = Type.CAN) public static final int FRONT_RIGHT_DRIVE = 8;
27+
@Port(type = Type.CAN) public static final int FRONT_RIGHT_PIVOT = 9;
28+
@Port(type = Type.CAN) public static final int BACK_LEFT_DRIVE = 12;
29+
@Port(type = Type.CAN) public static final int BACK_LEFT_PIVOT = 13;
30+
@Port(type = Type.CAN) public static final int BACK_RIGHT_DRIVE = 6;
31+
@Port(type = Type.CAN) public static final int BACK_RIGHT_PIVOT = 7;
32+
33+
@Port(type= Type.ANALOG) public static final int FRONT_LEFT_ENCODER = 0;
34+
@Port(type= Type.ANALOG) public static final int FRONT_RIGHT_ENCODER = 1;
35+
@Port(type= Type.ANALOG) public static final int BACK_LEFT_ENCODER = 2;
36+
@Port(type= Type.ANALOG) public static final int BACK_RIGHT_ENCODER = 3;
2337
}
2438
}

‎src/main/java/org/usfirst/frc/team69/robot/commands/AutonomousCommands.java‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
package org.usfirst.frc.team69.robot.commands;
2+
3+
import com.pathplanner.lib.auto.AutoBuilder;
4+
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
5+
import edu.wpi.first.wpilibj2.command.Command;
6+
7+
public class AutonomousCommands {
8+
9+
public SendableChooser<Command> createAutoChooser() {
10+
return AutoBuilder.buildAutoChooser();
11+
}
12+
13+
}

‎src/main/java/org/usfirst/frc/team69/robot/commands/SwerveJoystickCommand.java‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1 +1,84 @@
1+
package org.usfirst.frc.team69.robot.commands;
2+
3+
import edu.wpi.first.math.filter.SlewRateLimiter;
4+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
5+
import edu.wpi.first.math.kinematics.SwerveModuleState;
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
import org.usfirst.frc.team69.robot.subsystems.swerve.Constants.DriveConstants;
8+
import org.usfirst.frc.team69.robot.subsystems.swerve.Constants.OIConstants;
9+
import org.usfirst.frc.team69.robot.subsystems.swerve.SwerveSubsystem;
10+
11+
import java.util.function.Supplier;
12+
13+
114
public class SwerveJoystickCommand extends Command {
15+
16+
private final SwerveSubsystem swerveSubsystem;
17+
private final Supplier<Double> xSpdFunction, ySpdFunction, turningSpdFunction;
18+
private final Supplier<Boolean> fieldOrientedFunction;
19+
private final SlewRateLimiter xLimiter, yLimiter, turningLimiter;
20+
21+
public SwerveJoystickCommand(SwerveSubsystem swerveSubsystem,
22+
Supplier<Double> xSpdFunction, Supplier<Double> ySpdFunction, Supplier<Double> turningSpdFunction,
23+
Supplier<Boolean> fieldOrientedFunction) {
24+
this.swerveSubsystem = swerveSubsystem;
25+
this.xSpdFunction = xSpdFunction;
26+
this.ySpdFunction = ySpdFunction;
27+
this.turningSpdFunction = turningSpdFunction;
28+
this.fieldOrientedFunction = fieldOrientedFunction;
29+
this.xLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond);
30+
this.yLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond);
31+
this.turningLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAngularAccelerationUnitsPerSecond);
32+
addRequirements(swerveSubsystem);
33+
}
34+
35+
@Override
36+
public void initialize() {
37+
}
38+
39+
@Override
40+
public void execute() {
41+
// 1. Get real-time joystick inputs
42+
double xSpeed = xSpdFunction.get();
43+
double ySpeed = ySpdFunction.get();
44+
double turningSpeed = turningSpdFunction.get();
45+
46+
// 2. Apply deadband
47+
xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0;
48+
ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0;
49+
turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0;
50+
51+
// 3. Make the driving smoother
52+
xSpeed = xLimiter.calculate(xSpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond;
53+
ySpeed = yLimiter.calculate(ySpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond;
54+
turningSpeed = turningLimiter.calculate(turningSpeed)
55+
* DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond;
56+
57+
// 4. Construct desired chassis speeds
58+
ChassisSpeeds chassisSpeeds;
59+
if (fieldOrientedFunction.get()) {
60+
// Relative to field
61+
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
62+
xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d());
63+
} else {
64+
// Relative to robot
65+
chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed);
66+
}
67+
68+
// 5. Convert chassis speeds to individual module states
69+
SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds);
70+
71+
// 6. Output each module states to wheels
72+
swerveSubsystem.setModuleStates(moduleStates);
73+
}
74+
75+
@Override
76+
public void end(boolean interrupted) {
77+
swerveSubsystem.stopModules();
78+
}
79+
80+
@Override
81+
public boolean isFinished() {
82+
return false;
83+
}
84+
}

0 commit comments

Comments
 (0)