Skip to content

Commit 1fc6afb

Browse files
Wisdo1Wisdo1
committed
Looked through Encoder formulas
Added automation files
1 parent 60e2b1d commit 1fc6afb

File tree

4 files changed

+295
-6
lines changed

4 files changed

+295
-6
lines changed

‎src/main/java/frc/robot/subsystems/drive/Drive.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -266,7 +266,7 @@ private SwerveModulePosition[] getModulePositions() {
266266

267267
/** Returns the measured chassis speeds of the robot. */
268268
@AutoLogOutput(key = "SwerveChassisSpeeds/Measured")
269-
private ChassisSpeeds getChassisSpeeds() {
269+
public ChassisSpeeds getChassisSpeeds() {
270270
return kinematics.toChassisSpeeds(getModuleStates());
271271
}
272272

@@ -322,4 +322,5 @@ public double getMaxLinearSpeedMetersPerSec() {
322322
public double getMaxAngularSpeedRadPerSec() {
323323
return maxSpeedMetersPerSec / driveBaseRadius;
324324
}
325+
325326
}

‎src/main/java/frc/robot/subsystems/drive/DriveConstants.java‎

Lines changed: 32 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,20 @@
1515

1616
import static edu.wpi.first.units.Units.Inches;
1717
import static edu.wpi.first.units.Units.KilogramSquareMeters;
18+
import static edu.wpi.first.units.Units.Seconds;
1819
import static edu.wpi.first.units.Units.Volts;
1920

2021
import com.pathplanner.lib.config.ModuleConfig;
2122
import com.pathplanner.lib.config.RobotConfig;
23+
import com.pathplanner.lib.path.PathConstraints;
24+
2225
import edu.wpi.first.math.geometry.Pose2d;
2326
import edu.wpi.first.math.geometry.Rotation2d;
2427
import edu.wpi.first.math.geometry.Translation2d;
2528
import edu.wpi.first.math.system.plant.DCMotor;
2629
import edu.wpi.first.math.util.Units;
30+
import edu.wpi.first.units.measure.Distance;
31+
import edu.wpi.first.units.measure.Time;
2732
import edu.wpi.first.units.measure.Voltage;
2833
import org.ironmaple.simulation.drivesims.COTS;
2934
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
@@ -70,11 +75,11 @@ public class DriveConstants {
7075

7176
// Drive encoder configration
7277
public static final double driveEncoderPositionFactor =
73-
// rot -> Rad -> wheel rad; * 2pi / gear ratio
78+
// rot -> Rad -> wheel rad; 2pi / gear ratio
7479
(2 * Math.PI) / 6.75; // Rotor Rotations -> Wheel Radians
7580
public static final double driveEncoderVelocityFactor =
76-
// rot -> Rad -> wheel rad; * 2pi / gear ratio
77-
(2 * Math.PI) / 6.75; // Rotor RPM -> Wheel Rad/Sec
81+
// rot -> Rad -> wheel rad; 2pi / gear ratio / seconds
82+
(2 * Math.PI) / 6.75 / 60.0; // Rotor RPM -> Wheel Rad/Sec
7883

7984
// Drive PID configuration
8085
public static final double driveKp = 0.0;
@@ -94,8 +99,8 @@ public class DriveConstants {
9499

95100
// Turn encoder configuration
96101
public static final boolean turnEncoderInverted = true;
97-
public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians
98-
public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec
102+
public static final double turnEncoderPositionFactor = 2 * Math.PI/turnMotorReduction; // Rotations -> WHEEL Radians
103+
public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0/turnMotorReduction; // RPM -> WHEEL Rad/Sec
99104

100105
// Turn PID configuration
101106
public static final double turnKp = 0.8;
@@ -122,6 +127,28 @@ public class DriveConstants {
122127
driveMotorCurrentLimit,
123128
1),
124129
moduleTranslations);
130+
131+
// Path On The Fly Constraints
132+
public static PathConstraints kTeleopPathConstraints =
133+
new PathConstraints(
134+
Units.feetToMeters(15/4), 4.3/4, Units.degreesToRadians(540/4), Units.degreesToRadians(720/4));
135+
// All divided by four in case
136+
137+
public static PathConstraints kAutoPathConstraints =
138+
new PathConstraints(
139+
Units.feetToMeters(6), 1.0, Units.degreesToRadians(540), Units.degreesToRadians(540));
140+
141+
// AUTO Align
142+
public static Time kEndTriggerDebounce = Time.ofBaseUnits(1, Seconds);
143+
144+
public static Time kTeleopAlignAdjustTimeout = Time.ofBaseUnits(10, Seconds);
145+
public static Time kAutoAlignAdjustTimeout = Time.ofBaseUnits(3, Seconds);
146+
147+
public static Distance kPositionTolerance = Distance.ofBaseUnits(1, Inches);
148+
public static Distance kSpeedTolerance = Distance.ofBaseUnits(1, Inches);
149+
150+
public static Rotation2d kRotationTolerance = Rotation2d.fromDegrees(1);
151+
125152

126153
public static DriveTrainSimulationConfig mapleSimConfig =
127154
DriveTrainSimulationConfig.Default()

‎src/main/java/frc/robot/util/DriveTo.java‎

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
package frc.robot.util;
2+
3+
import static edu.wpi.first.units.Units.MetersPerSecond;
4+
import static frc.robot.subsystems.drive.DriveConstants.*;
5+
6+
import com.pathplanner.lib.auto.AutoBuilder;
7+
import com.pathplanner.lib.path.GoalEndState;
8+
import com.pathplanner.lib.path.IdealStartingState;
9+
import com.pathplanner.lib.path.PathPlannerPath;
10+
import com.pathplanner.lib.path.Waypoint;
11+
import edu.wpi.first.apriltag.AprilTagFieldLayout;
12+
import edu.wpi.first.math.geometry.Pose2d;
13+
import edu.wpi.first.math.geometry.Rotation2d;
14+
import edu.wpi.first.math.geometry.Translation2d;
15+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
16+
import edu.wpi.first.units.measure.LinearVelocity;
17+
import edu.wpi.first.wpilibj.DriverStation;
18+
import edu.wpi.first.wpilibj2.command.Command;
19+
import edu.wpi.first.wpilibj2.command.Commands;
20+
import frc.robot.subsystems.drive.Drive;
21+
22+
23+
import java.util.List;
24+
import java.util.Set;
25+
26+
/**
27+
* A Util Class for making path on the fly for all auto movement with vision or object detection in
28+
* the future
29+
*/
30+
public class DriveTo {
31+
32+
public boolean isPIDLoopRunning = false;
33+
34+
private final Drive mSwerve;
35+
36+
public DriveTo(Drive mSwerve, AprilTagFieldLayout field) {
37+
this.mSwerve = mSwerve;
38+
}
39+
40+
/**
41+
* Actual Path On-the-fly Command Start from here
42+
*
43+
* @param waypoint
44+
* @return
45+
*/
46+
public Command getPathFromWaypoint(Pose2d waypoint) {
47+
List<Waypoint> waypoints =
48+
PathPlannerPath.waypointsFromPoses(
49+
new Pose2d(
50+
mSwerve.getPose().getTranslation(),
51+
new Rotation2d(
52+
mSwerve.getChassisSpeeds().vxMetersPerSecond,
53+
mSwerve.getChassisSpeeds().vyMetersPerSecond)
54+
));
55+
56+
if (waypoints.get(0).anchor().getDistance(waypoints.get(1).anchor()) < 0.01) {
57+
return Commands.sequence(
58+
Commands.print("start position PID loop"),
59+
PositionPIDCommand.generateCommand(mSwerve, waypoint, kAutoAlignAdjustTimeout, true),
60+
Commands.print("end position PID loop"));
61+
}
62+
63+
PathPlannerPath path =
64+
new PathPlannerPath(
65+
waypoints,
66+
DriverStation.isAutonomous() ? kAutoPathConstraints : kTeleopPathConstraints,
67+
68+
new IdealStartingState(
69+
MetersPerSecond.of(
70+
new Translation2d(
71+
mSwerve.getChassisSpeeds().vxMetersPerSecond,
72+
mSwerve.getChassisSpeeds().vyMetersPerSecond).getNorm()),
73+
mSwerve.getRotation()),
74+
75+
new GoalEndState(0.0, waypoint.getRotation()));
76+
77+
path.preventFlipping = true;
78+
79+
return (AutoBuilder.followPath(path)
80+
.andThen(
81+
Commands.print("start position PID loop"),
82+
PositionPIDCommand.generateCommand(
83+
mSwerve,
84+
waypoint,
85+
(DriverStation.isAutonomous()
86+
? kAutoAlignAdjustTimeout
87+
: kTeleopAlignAdjustTimeout),
88+
DriverStation.isAutonomous() ? true : false) // for teleop at least
89+
.beforeStarting(
90+
Commands.runOnce(
91+
() -> {
92+
isPIDLoopRunning = true;
93+
}))
94+
.finallyDo(
95+
() -> {
96+
isPIDLoopRunning = false;
97+
})))
98+
.finallyDo(
99+
(interupt) -> {
100+
if (interupt) { // if this is false then the position pid would've X braked & called
101+
// the same method
102+
Commands.none(); // does nothing so we can keep driving
103+
}
104+
});
105+
}
106+
107+
/** Will go straight to the given pose*/
108+
public Command generateCommand(Pose2d pose) {
109+
return Commands.defer(
110+
() -> {
111+
return getPathFromWaypoint(pose);
112+
},
113+
Set.of());
114+
}
115+
}

‎src/main/java/frc/robot/util/PositionPIDCommand.java‎

Lines changed: 146 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,146 @@
1+
/*Taken straight from 4915
2+
* https://github.com/Spartronics4915/2025-Reefscape/blob/unstable/src/main/java/com/spartronics4915/frc2025/commands/autos/PositionPIDCommand.java#L30
3+
*/
4+
package frc.robot.util;
5+
6+
import static edu.wpi.first.units.Units.Centimeter;
7+
import static edu.wpi.first.units.Units.Degrees;
8+
import static edu.wpi.first.units.Units.Meters;
9+
import static edu.wpi.first.units.Units.Seconds;
10+
import static frc.robot.subsystems.drive.DriveConstants.*;
11+
12+
import com.pathplanner.lib.config.PIDConstants;
13+
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
14+
import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState;
15+
import edu.wpi.first.math.MathUtil;
16+
import edu.wpi.first.math.geometry.Pose2d;
17+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
18+
import edu.wpi.first.networktables.BooleanPublisher;
19+
import edu.wpi.first.networktables.DoublePublisher;
20+
import edu.wpi.first.networktables.NetworkTableInstance;
21+
import edu.wpi.first.units.measure.Time;
22+
import edu.wpi.first.wpilibj.Timer;
23+
import edu.wpi.first.wpilibj2.command.Command;
24+
import edu.wpi.first.wpilibj2.command.button.Trigger;
25+
import frc.robot.subsystems.drive.Drive;
26+
27+
import java.util.function.BooleanSupplier;
28+
29+
public class PositionPIDCommand extends Command {
30+
31+
public Drive mSwerve;
32+
public final Pose2d goalPose;
33+
private PPHolonomicDriveController mDriveController =
34+
new PPHolonomicDriveController(new PIDConstants(1),new PIDConstants(1)); //NOT TUNED
35+
// PPHolonomicController is the built in path following controller for holonomic drive trains;
36+
37+
private final Trigger endTrigger;
38+
private final Trigger endTriggerDebounced;
39+
40+
private final Timer timer = new Timer();
41+
42+
private final BooleanPublisher endTriggerLogger =
43+
NetworkTableInstance.getDefault()
44+
.getTable("logging")
45+
.getBooleanTopic("PositionPIDEndTrigger")
46+
.publish();
47+
private final DoublePublisher xErrLogger =
48+
NetworkTableInstance.getDefault().getTable("logging").getDoubleTopic("X Error").publish();
49+
private final DoublePublisher yErrLogger =
50+
NetworkTableInstance.getDefault().getTable("logging").getDoubleTopic("Y Error").publish();
51+
52+
private final BooleanSupplier booleanSupplier;
53+
54+
private PositionPIDCommand(Drive mSwerve, Pose2d goalPose) {
55+
this.mSwerve = mSwerve;
56+
this.goalPose = goalPose;
57+
58+
booleanSupplier =
59+
() -> {
60+
Pose2d diff = mSwerve.getPose().relativeTo(goalPose);
61+
62+
var rotation =
63+
MathUtil.isNear(
64+
0.0,
65+
diff.getRotation().getRotations(),
66+
kRotationTolerance.getRotations(),
67+
0.0,
68+
1.0);
69+
70+
var position = diff.getTranslation().getNorm() < kPositionTolerance.in(Meters);
71+
72+
// TODO: ADD SPEED FUNCTION
73+
// var speed = mSwerve.getSpeed() < kSpeedTolerance.in(MetersPerSecond);
74+
75+
// System.out.println("end trigger conditions R: "+ rotation + "\tP: " + position + "\tS:
76+
// " + speed);
77+
78+
return rotation && position; // && speed;
79+
};
80+
81+
endTrigger = new Trigger(booleanSupplier);
82+
83+
endTriggerDebounced = endTrigger.debounce(kEndTriggerDebounce.in(Seconds));
84+
}
85+
86+
public static Command generateCommand(
87+
Drive swerve, Pose2d goalPose, Time timeout, Boolean freeze) {
88+
return new PositionPIDCommand(swerve, goalPose)
89+
.withTimeout(timeout)
90+
.finallyDo(
91+
() -> {
92+
if (freeze) {
93+
swerve.stopWithX();
94+
}
95+
});
96+
}
97+
98+
@Override
99+
public void initialize() {
100+
endTriggerLogger.accept(endTrigger.getAsBoolean());
101+
timer.restart();
102+
}
103+
104+
@Override
105+
public void execute() {
106+
PathPlannerTrajectoryState goalState = new PathPlannerTrajectoryState();
107+
goalState.pose = goalPose;
108+
109+
endTriggerLogger.accept(endTrigger.getAsBoolean());
110+
ChassisSpeeds speeds =
111+
mDriveController.calculateRobotRelativeSpeeds(mSwerve.getPose(), goalState);
112+
mSwerve.runVelocity(speeds);
113+
114+
xErrLogger.accept(mSwerve.getPose().getX() - goalPose.getX());
115+
yErrLogger.accept(mSwerve.getPose().getY() - goalPose.getY());
116+
}
117+
118+
@Override
119+
public void end(boolean interrupted) {
120+
endTriggerLogger.accept(endTrigger.getAsBoolean());
121+
timer.stop();
122+
123+
Pose2d diff = mSwerve.getPose().relativeTo(goalPose);
124+
125+
System.out.println(
126+
"Adjustments to alginment took: "
127+
+ timer.get()
128+
+ " seconds and interrupted = "
129+
+ interrupted
130+
+ "\nPosition offset: "
131+
+ Centimeter.convertFrom(diff.getTranslation().getNorm(), Meters)
132+
+ " cm"
133+
+ "\nRotation offset: "
134+
+ diff.getRotation().getMeasure().in(Degrees)
135+
+ " deg"
136+
+ "\nVelocity value: "
137+
+ "Speed Not Implemented" // mSwerve.getSpeed() + "m/s"
138+
// We dont have a get speed function yet
139+
);
140+
}
141+
142+
@Override
143+
public boolean isFinished() {
144+
return endTriggerDebounced.getAsBoolean();
145+
}
146+
}

0 commit comments

Comments
 (0)