Skip to content

Commit

Permalink
chore: Reorganize drive
Browse files Browse the repository at this point in the history
  • Loading branch information
@msoucy
msoucy committed Jan 23, 2025
1 parent 1c1cc73 commit 08bcc66
Showing 1 changed file with 50 additions and 64 deletions.
114 changes: 50 additions & 64 deletions src/main/java/frc/robot/subsystems/Drive.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package frc.robot.subsystems;

import java.util.Optional;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;

Expand All @@ -13,15 +12,12 @@
import com.chopshop166.chopshoplib.maps.VisionMap;
import com.chopshop166.chopshoplib.motors.Modifier;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
Expand All @@ -48,9 +44,9 @@ public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
ProfiledPIDController rotationPID = new ProfiledPIDController(0.065, 0.0, 0.0, new Constraints(240, 270));
double visionMaxError = 1;

DoubleSupplier xSpeed;
DoubleSupplier ySpeed;
DoubleSupplier rotation;
DoubleSupplier xSpeedSupplier;
DoubleSupplier ySpeedSupplier;
DoubleSupplier rotationSupplier;
boolean isRobotCentric = false;

SwerveDrivePoseEstimator estimator;
Expand Down Expand Up @@ -87,9 +83,9 @@ public Drive(SwerveDriveMap map, DoubleSupplier xSpeed, DoubleSupplier ySpeed, D

rotationPID.enableContinuousInput(-180, 180);

this.xSpeed = xSpeed;
this.ySpeed = ySpeed;
this.rotation = rotation;
this.xSpeedSupplier = xSpeed;
this.ySpeedSupplier = ySpeed;
this.rotationSupplier = rotation;

}

Expand All @@ -104,50 +100,6 @@ public Command setPoseCommand(Supplier<Pose2d> pose) {
});
}

private void periodicMove(final double xSpeed, final double ySpeed,
final double rotation, boolean isRobotCentric) {

var deadband = Modifier.scalingDeadband(0.05);
double rotationInput = deadband.applyAsDouble(rotation);
double xInput = deadband.applyAsDouble(xSpeed);
double yInput = deadband.applyAsDouble(ySpeed);

final double translateXSpeed = xInput
* maxDriveSpeedMetersPerSecond * speedCoef;
final double translateYSpeed = yInput
* maxDriveSpeedMetersPerSecond * speedCoef;
double rotationSpeed = rotationInput
* maxRotationRadiansPerSecond * rotationCoef;

move(translateXSpeed, translateYSpeed, rotationSpeed, isRobotCentric);
}

private void move(final double xSpeed, final double ySpeed,
final double rotation, boolean isRobotCentric) {

// rotationOffset is temporary and startingRotation is set at the start
ChassisSpeeds speeds;
if (isRobotCentric) {
speeds = new ChassisSpeeds(ySpeed, xSpeed, rotation);
} else {
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(ySpeed, xSpeed,
rotation, estimator.getEstimatedPosition().getRotation());
}

move(speeds);

}

private void move(final ChassisSpeeds speeds) {

// Now use this in our kinematics
final SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds);
SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxDriveSpeedMetersPerSecond);

// All the states
getData().setDesiredStates(moduleStates);
}

public ChassisSpeeds getSpeeds() {
return kinematics.toChassisSpeeds(getData().getModuleStates());
}
Expand All @@ -168,12 +120,6 @@ public Command moveInDirection(double xSpeed, double ySpeed, double seconds) {
}

// Vision commands!!
public Command rotateToAngle(double targetAngle) {
return run(() -> {
rotateToAngleImpl(targetAngle);
});
}

public Translation2d getRobotToTarget(Translation2d target) {
return target.minus(estimator.getEstimatedPosition().getTranslation());
}
Expand All @@ -186,7 +132,6 @@ public Command rotateTo(Supplier<Translation2d> target) {
return run(() -> {
double rotationSpeed = calculateRotateSpeedToTarget(target);
move(0, 0, rotationSpeed, false);

});
}

Expand All @@ -213,7 +158,7 @@ public Command rotateToReefCenter() {
public double calculateRotateSpeedToTarget(Supplier<Translation2d> target) {
var robotToTarget = getRobotToTarget(target.get());
Logger.recordOutput("Target Pose", robotToTarget);
return rotateToAngleImpl(robotToTarget.getAngle().getDegrees());
return calculateRotationSpeed(robotToTarget.getAngle().getDegrees());
}

@Override
Expand All @@ -237,14 +182,15 @@ public void periodic() {

visionMap.updateData(estimator);

periodicMove(xSpeed.getAsDouble(), ySpeed.getAsDouble(), rotation.getAsDouble(), isRobotCentric);
periodicMove(xSpeedSupplier.getAsDouble(), ySpeedSupplier.getAsDouble(), rotationSupplier.getAsDouble(),
isRobotCentric);

Logger.recordOutput("Estimator Pose", estimator.getEstimatedPosition());
Logger.recordOutput("Pose Angle", estimator.getEstimatedPosition().getRotation());
Logger.recordOutput("Robot Rotation Gyro", getMap().gyro.getRotation2d());
}

private double rotateToAngleImpl(double targetAngleDegrees) {
private double calculateRotationSpeed(double targetAngleDegrees) {
double estimatorAngle = estimator.getEstimatedPosition().getRotation().getDegrees();
double rotationSpeed = rotationPID.calculate(estimatorAngle, targetAngleDegrees);
rotationSpeed += Math.copySign(rotationKs, rotationSpeed);
Expand All @@ -259,4 +205,44 @@ private double rotateToAngleImpl(double targetAngleDegrees) {
Logger.recordOutput("Position Error", rotationPID.getPositionError());
return rotationSpeed;
}

private void periodicMove(final double xSpeed, final double ySpeed,
final double rotation, boolean isRobotCentric) {
var deadband = Modifier.scalingDeadband(0.05);
double rotationInput = deadband.applyAsDouble(rotation);
double xInput = deadband.applyAsDouble(xSpeed);
double yInput = deadband.applyAsDouble(ySpeed);

final double translateXSpeed = xInput
* maxDriveSpeedMetersPerSecond * speedCoef;
final double translateYSpeed = yInput
* maxDriveSpeedMetersPerSecond * speedCoef;
double rotationSpeed = rotationInput
* maxRotationRadiansPerSecond * rotationCoef;

move(translateXSpeed, translateYSpeed, rotationSpeed, isRobotCentric);
}

private void move(final double xSpeed, final double ySpeed,
final double rotation, boolean isRobotCentric) {
// rotationOffset is temporary and startingRotation is set at the start
ChassisSpeeds speeds;
if (isRobotCentric) {
speeds = new ChassisSpeeds(ySpeed, xSpeed, rotation);
} else {
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(ySpeed, xSpeed,
rotation, estimator.getEstimatedPosition().getRotation());
}

move(speeds);
}

private void move(final ChassisSpeeds speeds) {
// Now use this in our kinematics
final SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds);
SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxDriveSpeedMetersPerSecond);

// All the states
getData().setDesiredStates(moduleStates);
}
}

0 comments on commit 08bcc66

Please sign in to comment.