Skip to content

Commit 3543661

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #3 from strykeforce/auton
Initial Drive Subsystem and Path Handler
2 parents 74134e7 + 6fe746c commit 3543661

File tree

13 files changed

+1398
-26
lines changed

13 files changed

+1398
-26
lines changed

‎.vscode/settings.json‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,5 +56,6 @@
5656
"edu.wpi.first.math.proto.*",
5757
"edu.wpi.first.math.**.proto.*",
5858
"edu.wpi.first.math.**.struct.*",
59-
]
59+
],
60+
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
6061
}

‎src/main/deploy/choreo/2025-project.chor‎

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -9,22 +9,22 @@
99
"config":{
1010
"frontLeft":{
1111
"x":{
12-
"exp":"11 in",
13-
"val":0.2794
12+
"exp":"12.125 in",
13+
"val":0.307975
1414
},
1515
"y":{
16-
"exp":"11 in",
17-
"val":0.2794
16+
"exp":"12.125 in",
17+
"val":0.307975
1818
}
1919
},
2020
"backLeft":{
2121
"x":{
22-
"exp":"-11 in",
23-
"val":-0.2794
22+
"exp":"-12.125 in",
23+
"val":-0.307975
2424
},
2525
"y":{
26-
"exp":"11 in",
27-
"val":0.2794
26+
"exp":"12.125 in",
27+
"val":0.307975
2828
}
2929
},
3030
"mass":{
@@ -40,8 +40,8 @@
4040
"val":6.5
4141
},
4242
"radius":{
43-
"exp":"2 in",
44-
"val":0.0508
43+
"exp":"1.75 in",
44+
"val":0.04445
4545
},
4646
"vmax":{
4747
"exp":"6000 RPM",
@@ -52,21 +52,21 @@
5252
"val":1.2
5353
},
5454
"cof":{
55-
"exp":"1.5",
56-
"val":1.5
55+
"exp":"1.1",
56+
"val":1.1
5757
},
5858
"bumper":{
5959
"front":{
60-
"exp":"16 in",
61-
"val":0.4064
60+
"exp":"35.875 in",
61+
"val":0.911225
6262
},
6363
"side":{
64-
"exp":"16 in",
65-
"val":0.4064
64+
"exp":"35.875 in",
65+
"val":0.911225
6666
},
6767
"back":{
68-
"exp":"16 in",
69-
"val":0.4064
68+
"exp":"35.875 in",
69+
"val":0.911225
7070
}
7171
},
7272
"differentialTrackWidth":{

‎src/main/java/frc/robot/commands/auton/AutoCommandInterface.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
package frc.robot.commands.auton;
2+
3+
public interface AutoCommandInterface {
4+
public void reassignAlliance();
5+
}

‎src/main/java/frc/robot/commands/drive/DriveAutonCommand.java‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
package frc.robot.commands.drive;
2+
3+
import choreo.Choreo;
4+
import choreo.trajectory.SwerveSample;
5+
import choreo.trajectory.Trajectory;
6+
import edu.wpi.first.math.geometry.Pose2d;
7+
import edu.wpi.first.wpilibj.Timer;
8+
import edu.wpi.first.wpilibj2.command.Command;
9+
import frc.robot.commands.auton.AutoCommandInterface;
10+
import frc.robot.subsystems.drive.DriveSubsystem;
11+
import java.util.Optional;
12+
import org.slf4j.Logger;
13+
import org.slf4j.LoggerFactory;
14+
15+
public class DriveAutonCommand extends Command implements AutoCommandInterface {
16+
private final DriveSubsystem driveSubsystem;
17+
private Trajectory<SwerveSample> trajectory;
18+
private final Timer timer = new Timer();
19+
private static final Logger logger = LoggerFactory.getLogger(DriveAutonCommand.class);
20+
private boolean isTherePath = false;
21+
private String trajectoryName;
22+
private boolean mirrorTrajectory = false;
23+
24+
private boolean resetOdometry;
25+
private boolean lastPath;
26+
27+
public DriveAutonCommand(
28+
DriveSubsystem driveSubsystem,
29+
String trajectoryName,
30+
boolean lastPath,
31+
boolean resetOdometry) {
32+
33+
addRequirements(driveSubsystem);
34+
this.driveSubsystem = driveSubsystem;
35+
this.resetOdometry = resetOdometry;
36+
this.lastPath = lastPath;
37+
this.trajectoryName = trajectoryName;
38+
Optional<Trajectory<SwerveSample>> tempTrajectory = Choreo.loadTrajectory(trajectoryName);
39+
if (tempTrajectory.isPresent()) {
40+
trajectory = tempTrajectory.get();
41+
isTherePath = true;
42+
} else {
43+
logger.error("Trajectory {} not found", trajectoryName);
44+
isTherePath = false;
45+
}
46+
timer.start();
47+
}
48+
49+
@Override
50+
public void reassignAlliance() {
51+
mirrorTrajectory = driveSubsystem.shouldFlip();
52+
}
53+
54+
@Override
55+
public void initialize() {
56+
if (isTherePath) {
57+
driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName);
58+
Pose2d initialPose = new Pose2d();
59+
initialPose = trajectory.getInitialPose(mirrorTrajectory).get();
60+
if (resetOdometry) {
61+
driveSubsystem.resetOdometry(initialPose);
62+
}
63+
driveSubsystem.setEnableHolo(true);
64+
// driveSubsystem.recordAutoTrajectory(trajectory);
65+
driveSubsystem.resetHolonomicController();
66+
driveSubsystem.grapherTrajectoryActive(true);
67+
timer.reset();
68+
logger.info("Begin Trajectory: {}", trajectoryName);
69+
SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
70+
driveSubsystem.calculateController(desiredState);
71+
}
72+
}
73+
74+
@Override
75+
public void execute() {
76+
if (isTherePath) {
77+
SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
78+
driveSubsystem.calculateController(desiredState);
79+
}
80+
}
81+
82+
@Override
83+
public boolean isFinished() {
84+
if (!isTherePath) {
85+
return false;
86+
}
87+
return timer.hasElapsed(trajectory.getTotalTime());
88+
}
89+
90+
@Override
91+
public void end(boolean interrupted) {
92+
driveSubsystem.setEnableHolo(false);
93+
// driveSubsystem.recordAutoTrajectory(null);
94+
95+
if (!interrupted && !lastPath) {
96+
driveSubsystem.calculateController(
97+
trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get());
98+
} else {
99+
driveSubsystem.drive(0, 0, 0);
100+
}
101+
102+
driveSubsystem.grapherTrajectoryActive(false);
103+
logger.info("End Trajectory {}: {}", trajectoryName, timer.get());
104+
driveSubsystem.setAutoDebugMsg("End " + trajectoryName);
105+
}
106+
}

‎src/main/java/frc/robot/constants/DriveConstants.java‎

Lines changed: 149 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,151 @@
11
package frc.robot.constants;
22

3-
public class DriveConstants {}
3+
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
4+
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
5+
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
6+
import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod;
7+
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
8+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
9+
import com.ctre.phoenix6.configs.Pigeon2Configuration;
10+
import com.ctre.phoenix6.configs.Slot0Configs;
11+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
12+
import com.ctre.phoenix6.signals.NeutralModeValue;
13+
import edu.wpi.first.math.geometry.Translation2d;
14+
15+
public class DriveConstants {
16+
public static final double kDriveGearRatio = 6.5;
17+
public static final double kWheelDiameterInches = 4.0;
18+
public static final double kMaxSpeedMetersPerSecond = 12.0;
19+
public static final double kSpeedStillThreshold = 0.1; // meters per second
20+
public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second
21+
public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
22+
public static final int kGyroDifferentCount = 3;
23+
24+
public static final double kRobotLength = 22.0;
25+
public static final double kRobotWidth = 22.0;
26+
public static final double kFieldMaxX = 690.0;
27+
28+
public static final double kPOmega = 4.5;
29+
public static final double kIOmega = 0.0;
30+
public static final double kDOmega = 0.0;
31+
public static final double kMaxVelOmega =
32+
(kMaxSpeedMetersPerSecond / Math.hypot(kRobotWidth / 2.0, kRobotLength / 2.0)) / 2.0;
33+
public static final double kMaxAccelOmega = 5.0;
34+
35+
public static final double kPHolonomic = 3.0; // was 3
36+
public static final double kIHolonomic = 0.0000;
37+
public static final double kDHolonomic = 0.00; // kPHolonomic/100
38+
39+
public static Translation2d[] getWheelLocationMeters() {
40+
final double x = kRobotLength / 2.0; // front-back, was ROBOT_LENGTH
41+
final double y = kRobotWidth / 2.0; // left-right, was ROBOT_WIDTH
42+
Translation2d[] locs = new Translation2d[4];
43+
locs[0] = new Translation2d(x, y); // left front
44+
locs[1] = new Translation2d(x, -y); // right front
45+
locs[2] = new Translation2d(-x, y); // left rear
46+
locs[3] = new Translation2d(-x, -y); // right rear
47+
return locs;
48+
}
49+
50+
// temp stuff
51+
public static final int kTempAvgCount = 25;
52+
public static final double kTripTemp = 1300;
53+
public static final double kRecoverTemp = 1290;
54+
public static final double kNotifyTemp = 1295;
55+
56+
public static TalonSRXConfiguration
57+
getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
58+
// constructor sets encoder to Quad/CTRE_MagEncoder_Relative
59+
TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration();
60+
61+
azimuthConfig.primaryPID.selectedFeedbackCoefficient = 1.0;
62+
azimuthConfig.auxiliaryPID.selectedFeedbackSensor = FeedbackDevice.None;
63+
64+
azimuthConfig.forwardLimitSwitchSource = LimitSwitchSource.Deactivated;
65+
azimuthConfig.reverseLimitSwitchSource = LimitSwitchSource.Deactivated;
66+
67+
azimuthConfig.continuousCurrentLimit = 10;
68+
azimuthConfig.peakCurrentDuration = 0;
69+
azimuthConfig.peakCurrentLimit = 0;
70+
71+
azimuthConfig.slot0.kP = 15.0;
72+
azimuthConfig.slot0.kI = 0.0;
73+
azimuthConfig.slot0.kD = 150.0;
74+
azimuthConfig.slot0.kF = 1.0;
75+
azimuthConfig.slot0.integralZone = 0;
76+
azimuthConfig.slot0.allowableClosedloopError = 0;
77+
azimuthConfig.slot0.maxIntegralAccumulator = 0;
78+
79+
azimuthConfig.motionCruiseVelocity = 800;
80+
azimuthConfig.motionAcceleration = 10_000;
81+
azimuthConfig.velocityMeasurementWindow = 64;
82+
azimuthConfig.velocityMeasurementPeriod = SensorVelocityMeasPeriod.Period_100Ms;
83+
azimuthConfig.voltageCompSaturation = 12;
84+
azimuthConfig.voltageMeasurementFilter = 32;
85+
azimuthConfig.neutralDeadband = 0.04;
86+
return azimuthConfig;
87+
}
88+
89+
public static TalonFXConfiguration getDriveTalonConfig() {
90+
TalonFXConfiguration driveConfig = new TalonFXConfiguration();
91+
92+
CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
93+
currentConfig.SupplyCurrentLimit = 60;
94+
95+
currentConfig.StatorCurrentLimit = 140;
96+
97+
currentConfig.SupplyCurrentLimitEnable = true;
98+
currentConfig.StatorCurrentLimitEnable = true;
99+
100+
driveConfig.CurrentLimits = currentConfig;
101+
102+
Slot0Configs slot0Config = new Slot0Configs();
103+
slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate
104+
slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate
105+
slot0Config.kD = 0.0;
106+
slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate
107+
driveConfig.Slot0 = slot0Config;
108+
109+
MotorOutputConfigs motorConfigs = new MotorOutputConfigs();
110+
motorConfigs.DutyCycleNeutralDeadband = 0.01;
111+
motorConfigs.NeutralMode = NeutralModeValue.Brake;
112+
driveConfig.MotorOutput = motorConfigs;
113+
114+
return driveConfig;
115+
}
116+
117+
public static final int kPigeonCanID = 1000; // fix later
118+
119+
public static Pigeon2Configuration getPigeon2Configuration() {
120+
Pigeon2Configuration config = new Pigeon2Configuration();
121+
122+
config.MountPose.MountPoseYaw = -90.0;
123+
config.MountPose.MountPoseRoll = 0.0;
124+
config.MountPose.MountPosePitch = 0.0;
125+
126+
config.GyroTrim.GyroScalarX = 0.0;
127+
config.GyroTrim.GyroScalarY = 0.0;
128+
config.GyroTrim.GyroScalarZ = -2.12;
129+
130+
return config;
131+
}
132+
133+
public static CurrentLimitsConfigs getSafeDriveLimits() {
134+
CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
135+
currentConfig.SupplyCurrentLimit = 30;
136+
currentConfig.SupplyCurrentLimitEnable = true;
137+
currentConfig.StatorCurrentLimitEnable = false;
138+
return currentConfig;
139+
}
140+
141+
public static CurrentLimitsConfigs getNormDriveLimits() {
142+
CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
143+
currentConfig.SupplyCurrentLimit = 30;
144+
145+
currentConfig.StatorCurrentLimit = 140;
146+
147+
currentConfig.SupplyCurrentLimitEnable = true;
148+
currentConfig.StatorCurrentLimitEnable = true;
149+
return currentConfig;
150+
}
151+
}

‎src/main/java/frc/robot/constants/RobotConstants.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
11
package frc.robot.constants;
22

3-
public class RobotConstants {}
3+
public class RobotConstants {
4+
public static final int kTalonConfigTimeout = 10; // ms
5+
}

‎src/main/java/frc/robot/constants/RobotStateConstants.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
11
package frc.robot.constants;
22

3-
public class RobotStateConstants {}
3+
public class RobotStateConstants {
4+
public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
5+
}

‎src/main/java/frc/robot/constants/TagServoingConstants.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
package frc.robot.constants;
2+
3+
public class TagServoingConstants {
4+
public static final double kAngleCloseEnough = 3.0;
5+
}

0 commit comments

Comments
 (0)