Skip to content

Commit 56da2f0

Browse files
HenryLi-0HenryLi-0
committed
all the drive stuff (including worlds!)
1 parent f1e44e9 commit 56da2f0

13 files changed

Lines changed: 361 additions & 145 deletions

‎src/main/java/org/sciborgs1155/robot/drive/Drive.java‎

Lines changed: 80 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
import static edu.wpi.first.units.Units.Seconds;
99
import static edu.wpi.first.units.Units.Volts;
1010
import static java.lang.Math.atan;
11-
import static org.sciborgs1155.lib.LoggingUtils.*;
11+
import static org.sciborgs1155.lib.LoggingUtils.log;
1212
import static org.sciborgs1155.robot.Constants.PERIOD;
1313
import static org.sciborgs1155.robot.Constants.TUNING;
1414
import static org.sciborgs1155.robot.Constants.allianceRotation;
@@ -51,7 +51,6 @@
5151
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
5252
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
5353
import java.util.Arrays;
54-
import java.util.DoubleSummaryStatistics;
5554
import java.util.List;
5655
import java.util.Optional;
5756
import java.util.concurrent.locks.ReentrantLock;
@@ -64,14 +63,12 @@
6463
import org.sciborgs1155.lib.InputStream;
6564
import org.sciborgs1155.lib.Tracer;
6665
import org.sciborgs1155.lib.Tuning;
67-
import org.sciborgs1155.robot.Constants;
68-
import org.sciborgs1155.robot.FieldConstants;
6966
import org.sciborgs1155.robot.Robot;
7067
import org.sciborgs1155.robot.drive.DriveConstants.Assisted;
7168
import org.sciborgs1155.robot.drive.DriveConstants.ControlMode;
7269
import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Driving;
70+
import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Turning;
7371
import org.sciborgs1155.robot.drive.DriveConstants.Rotation;
74-
import org.sciborgs1155.robot.drive.DriveConstants.Skid;
7572
import org.sciborgs1155.robot.drive.DriveConstants.Translation;
7673
import org.sciborgs1155.robot.vision.Vision.PoseEstimate;
7774

@@ -131,6 +128,7 @@ public class Drive extends SubsystemBase implements AutoCloseable {
131128
private final SwerveDrivePoseEstimator odometry;
132129

133130
private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
131+
private ChassisSpeeds prevSpeeds = new ChassisSpeeds();
134132

135133
// Faster Odometry
136134
private SwerveModulePosition[] lastPositions;
@@ -173,6 +171,8 @@ public static Drive create() {
173171
FRONT_LEFT_CANCODER,
174172
ANGULAR_OFFSETS.get(0),
175173
Driving.FF_CONSTANTS.get(0),
174+
Turning.FF_CONSTANTS.get(0),
175+
Turning.PID_CONSTANTS.get(0),
176176
"FL",
177177
false),
178178
new TalonModule(
@@ -181,14 +181,18 @@ public static Drive create() {
181181
FRONT_RIGHT_CANCODER,
182182
ANGULAR_OFFSETS.get(1),
183183
Driving.FF_CONSTANTS.get(1),
184+
Turning.FF_CONSTANTS.get(1),
185+
Turning.PID_CONSTANTS.get(1),
184186
"FR",
185-
false),
187+
true),
186188
new TalonModule(
187189
REAR_LEFT_DRIVE,
188190
REAR_LEFT_TURNING,
189191
REAR_LEFT_CANCODER,
190192
ANGULAR_OFFSETS.get(2),
191193
Driving.FF_CONSTANTS.get(2),
194+
Turning.FF_CONSTANTS.get(2),
195+
Turning.PID_CONSTANTS.get(2),
192196
"RL",
193197
false),
194198
new TalonModule(
@@ -197,8 +201,11 @@ public static Drive create() {
197201
REAR_RIGHT_CANCODER,
198202
ANGULAR_OFFSETS.get(3),
199203
Driving.FF_CONSTANTS.get(3),
204+
Turning.FF_CONSTANTS.get(3),
205+
Turning.PID_CONSTANTS.get(3),
200206
"RR",
201-
false));
207+
true));
208+
202209
case SPARK ->
203210
new Drive(
204211
new NavXGyro(),
@@ -207,27 +214,35 @@ public static Drive create() {
207214
FRONT_LEFT_TURNING,
208215
ANGULAR_OFFSETS.get(0),
209216
Driving.FF_CONSTANTS.get(0),
217+
Turning.FF_CONSTANTS.get(0),
218+
Turning.PID_CONSTANTS.get(0),
210219
"FL",
211220
true),
212221
new SparkModule(
213222
FRONT_RIGHT_DRIVE,
214223
FRONT_RIGHT_TURNING,
215224
ANGULAR_OFFSETS.get(1),
216225
Driving.FF_CONSTANTS.get(1),
226+
Turning.FF_CONSTANTS.get(1),
227+
Turning.PID_CONSTANTS.get(1),
217228
"FR",
218229
true),
219230
new SparkModule(
220231
REAR_LEFT_DRIVE,
221232
REAR_LEFT_TURNING,
222233
ANGULAR_OFFSETS.get(2),
223234
Driving.FF_CONSTANTS.get(2),
235+
Turning.FF_CONSTANTS.get(2),
236+
Turning.PID_CONSTANTS.get(2),
224237
"RL",
225238
true),
226239
new SparkModule(
227240
REAR_RIGHT_DRIVE,
228241
REAR_RIGHT_TURNING,
229242
ANGULAR_OFFSETS.get(3),
230243
Driving.FF_CONSTANTS.get(3),
244+
Turning.FF_CONSTANTS.get(3),
245+
Turning.PID_CONSTANTS.get(3),
231246
"RR",
232247
true));
233248
};
@@ -293,7 +308,9 @@ public Drive(
293308
new SysIdRoutine.Mechanism(
294309
volts ->
295310
modules.forEach(
296-
m -> m.updateInputs(Rotation2d.fromRadians(0), volts.in(Volts))),
311+
m ->
312+
m.updateInputsDrive(
313+
new SwerveModuleState(volts.in(Volts), Rotation2d.kZero))),
297314
null,
298315
this,
299316
"drive"));
@@ -306,14 +323,10 @@ public Drive(
306323
(state) -> SignalLogger.writeString("rotation state", state.toString())),
307324
new SysIdRoutine.Mechanism(
308325
volts -> {
309-
this.frontLeft.updateInputs(
310-
Rotation2d.fromRadians(3 * Math.PI / 4), volts.in(Volts));
311-
this.frontRight.updateInputs(
312-
Rotation2d.fromRadians(Math.PI / 4), volts.in(Volts));
313-
this.rearLeft.updateInputs(
314-
Rotation2d.fromRadians(-3 * Math.PI / 4), volts.in(Volts));
315-
this.rearRight.updateInputs(
316-
Rotation2d.fromRadians(-Math.PI / 4), volts.in(Volts));
326+
modules.forEach(
327+
module ->
328+
module.updateInputsTurn(
329+
new SwerveModuleState(0.0, Rotation2d.fromRadians(volts.in(Volts)))));
317330
},
318331
null,
319332
this,
@@ -374,9 +387,7 @@ public Drive(
374387
}
375388

376389
/**
377-
* Returns the currently-estimated pose of the robot.
378-
*
379-
* @return The pose.
390+
* @return The currently-estimated pose of the robot.
380391
*/
381392
@Logged
382393
public Pose2d pose() {
@@ -408,15 +419,15 @@ public double omega() {
408419
return fieldRelativeChassisSpeeds().omegaRadiansPerSecond;
409420
}
410421

411-
/** Returns a Pose3D of the estimated pose of the robot. */
422+
/**
423+
* @return A Pose3D of the estimated pose of the robot.
424+
*/
412425
public Pose3d pose3d() {
413426
return new Pose3d(odometry.getEstimatedPosition());
414427
}
415428

416429
/**
417-
* Returns the currently-estimated field-relative yaw of the robot.
418-
*
419-
* @return The rotation.
430+
* @return The currently-estimated field-relative yaw of the robot.
420431
*/
421432
@Logged
422433
public Rotation2d heading() {
@@ -464,7 +475,7 @@ public Command drive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vOmega
464475
vy.getAsDouble(),
465476
vOmega.getAsDouble(),
466477
heading().plus(allianceRotation())),
467-
ControlMode.OPEN_LOOP_VELOCITY));
478+
ControlMode.CLOSED_LOOP_VELOCITY));
468479
}
469480

470481
/**
@@ -652,7 +663,7 @@ public boolean atPose(Pose2d pose) {
652663
*/
653664
public void setChassisSpeeds(ChassisSpeeds desired, ControlMode mode) {
654665
desiredSpeeds = desired;
655-
ChassisSpeeds speeds = robotRelativeChassisSpeeds();
666+
ChassisSpeeds speeds = prevSpeeds;
656667
Vector<N2> currentVelocity =
657668
VecBuilder.fill(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
658669

@@ -670,13 +681,21 @@ public void setChassisSpeeds(ChassisSpeeds desired, ControlMode mode) {
670681
new ChassisSpeeds(
671682
limitedVelocity.get(0), limitedVelocity.get(1), desired.omegaRadiansPerSecond);
672683

684+
log(
685+
"/Robot/tuning/drive/changeInSpeeds",
686+
newSpeeds.minus(
687+
new ChassisSpeeds(
688+
currentVelocity.get(0), currentVelocity.get(1), desired.omegaRadiansPerSecond)),
689+
ChassisSpeeds.struct);
690+
673691
SwerveModuleState[] states = kinematics.toSwerveModuleStates(newSpeeds);
674692
SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_SPEED.in(MetersPerSecond));
675693
setModuleStates(
676694
kinematics.toSwerveModuleStates(
677-
ChassisSpeeds.discretize(
678-
kinematics.toChassisSpeeds(states), Constants.PERIOD.in(Seconds))),
695+
ChassisSpeeds.discretize(kinematics.toChassisSpeeds(states), PERIOD.in(Seconds))),
679696
mode);
697+
698+
prevSpeeds = newSpeeds;
680699
}
681700

682701
/**
@@ -695,7 +714,7 @@ private Vector<N2> forwardAccelerationLimit(Vector<N2> deltaV) {
695714
double limit =
696715
maxAccel.get()
697716
* PERIOD.in(Seconds)
698-
* (1 - Math.min(1, (currVel.norm() / MAX_SPEED.in(MetersPerSecond))));
717+
* (1 - Math.min(1, currVel.norm() / MAX_SPEED.in(MetersPerSecond)));
699718
log("/Robot/drive/accel limit", limit);
700719
Vector<N2> proj = deltaV.projection(currVel);
701720
if (proj.norm() > limit && proj.dot(currVel) > 0) {
@@ -769,6 +788,13 @@ public Command driveTo(Supplier<Pose2d> target) {
769788
.withName("drive to pose");
770789
}
771790

791+
/**
792+
* Command factory that automatically path-follows, in a straight line, to a position on the
793+
* field.
794+
*
795+
* @param goal The pose to reach.
796+
* @return The command to run the control loop until the pose is reached.
797+
*/
772798
public Command driveTo(Pose2d goal) {
773799
return driveTo(() -> goal);
774800
}
@@ -782,24 +808,25 @@ public double[] getWheelRadiusCharacterizationPositions() {
782808
return values;
783809
}
784810

785-
/**
786-
* @return If the robot is skidding.
787-
*/
788-
@Logged
789-
public boolean isSkidding() {
790-
DoubleSummaryStatistics diffs =
791-
Arrays.stream(moduleStates())
792-
.mapToDouble(
793-
s ->
794-
FieldConstants.fromPolarCoords(s.speedMetersPerSecond, s.angle)
795-
.minus(
796-
VecBuilder.fill(
797-
robotRelativeChassisSpeeds().vxMetersPerSecond,
798-
robotRelativeChassisSpeeds().vyMetersPerSecond))
799-
.norm())
800-
.summaryStatistics();
801-
return diffs.getMax() - diffs.getMin() > Skid.THRESHOLD.in(MetersPerSecond);
802-
}
811+
// TODO decide to split file, keep, or change/remove?
812+
// /**
813+
// * @return If the robot is skidding.
814+
// */
815+
// @Logged
816+
// public boolean isSkidding() {
817+
// DoubleSummaryStatistics diffs =
818+
// Arrays.stream(moduleStates())
819+
// .mapToDouble(
820+
// s ->
821+
// FieldConstants.fromPolarCoords(s.speedMetersPerSecond, s.angle)
822+
// .minus(
823+
// VecBuilder.fill(
824+
// robotRelativeChassisSpeeds().vxMetersPerSecond,
825+
// robotRelativeChassisSpeeds().vyMetersPerSecond))
826+
// .norm())
827+
// .summaryStatistics();
828+
// return diffs.getMax() - diffs.getMin() > Skid.THRESHOLD.in(MetersPerSecond);
829+
// }
803830

804831
/**
805832
* @return If the robot is colliding.
@@ -934,8 +961,8 @@ public void periodic() {
934961
try {
935962
double[] timestamps = modules.get(2).timestamps();
936963

937-
// get the positions of all modules at a given timestamp [[module0 odometry], [module1
938-
// odometry], ...]
964+
// get the positions of all modules at a given timestamp
965+
// [[module0 odometry], [module1 odometry], ...]
939966
SwerveModulePosition[][] allPositions = {
940967
modules.get(0).odometryData(),
941968
modules.get(1).odometryData(),
@@ -956,7 +983,7 @@ public void periodic() {
956983
lastPositions = modulePositions;
957984
lastHeading = angle;
958985
}
959-
} catch (Exception e) {
986+
} catch (RuntimeException e) {
960987
e.printStackTrace();
961988
} finally {
962989
LOCK.unlock();
@@ -966,6 +993,8 @@ public void periodic() {
966993
lastPositions = modulePositions();
967994
}
968995

996+
gyro.periodic();
997+
969998
// update our simulated field poses
970999
field2d.setRobotPose(pose());
9711000

@@ -995,8 +1024,7 @@ public void simulationPeriodic() {
9951024
simRotation.rotateBy(
9961025
Rotation2d.fromRadians(
9971026
!Double.isNaN(robotRelativeChassisSpeeds().omegaRadiansPerSecond)
998-
? robotRelativeChassisSpeeds().omegaRadiansPerSecond
999-
* Constants.PERIOD.in(Seconds)
1027+
? robotRelativeChassisSpeeds().omegaRadiansPerSecond * PERIOD.in(Seconds)
10001028
: 0));
10011029
}
10021030

@@ -1054,6 +1082,7 @@ public Command systemsCheck() {
10541082
.andThen(atAngle);
10551083
}
10561084

1085+
@Override
10571086
public void close() throws Exception {
10581087
frontLeft.close();
10591088
frontRight.close();

0 commit comments

Comments
 (0)