Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

fix elevator manual driving, and slow fine driving

Browse Source
This commit is contained in:
C4llSiqn
2025-02-28 17:31:04 -07:00
parent f0565d7844
commit 0c1ba8c294
3 changed files with 30 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+23 -18
View File
@@ -51,6 +51,7 @@ public class SwerveDrive extends Subsystem {
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
private boolean stopped = false;
private boolean robotKnowsWhereItIs = false;
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
@@ -59,6 +60,7 @@ public class SwerveDrive extends Subsystem {
public Pose2d initalPose2d = null;
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
@@ -170,24 +172,7 @@ public class SwerveDrive extends Subsystem {
SmartDashboard.putBoolean("drift correction", true);
}
// // SmartDashboard.putBoolean("drift correction", true);
// // rot = ((rotTarget - gyro.getAngle()) / 360) *
// SwerveDriveConstants.ROT_CORRECTION_SPEED;
// }
// // Use the left joystick to set speed. Apply a cubic curve and the set max
// speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00),
// Math.pow(speed.getY(), 3.00));
// // Convert field-relative speeds to robot-relative speeds.
// // chassisSpeeds = chassisSpeeds.
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 *
// speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction,
// gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
@@ -196,6 +181,16 @@ public class SwerveDrive extends Subsystem {
}
}
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
stopped = false;
// Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
}
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
// reason to have a robot
// relitive version of
@@ -291,6 +286,8 @@ public class SwerveDrive extends Subsystem {
public void resetGyro() {
swerveDriveTrain.tareEverything();
robotKnowsWhereItIs = false;
rotTarget = 0;
}
@@ -321,6 +318,14 @@ public class SwerveDrive extends Subsystem {
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
if (vision.isTag()) {
Pose2d pose = vision.getPose2d();
if (!robotKnowsWhereItIs) {
robotKnowsWhereItIs = true;
Pose2d currPose = getPose2d();
double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees();
rotTarget += deltaAngle;
}
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
}