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

Finish X position and PID position

Browse Source
This commit is contained in:
mimigamin
2026-03-14 18:57:04 -06:00
parent ebab028818
commit 8c8ac26139
7 changed files with 76 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java
+30 -36
View File
@@ -5,52 +5,46 @@
package frc4388.robot.commands.Swerve;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class StayInPosition extends PID {
SwerveDrive drive;
SwerveDrive drive;
public StayInPosition(SwerveDrive drive) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive;
addRequirements(drive);
}
/** Creates a new StayInPosition. */
public StayInPosition(SwerveDrive drive) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive;
public void goToTargetPose(Pose2d targetPose) {
Pose2d currentPose = drive.getCurrentPose();
addRequirements(drive);
}
double translationX = targetPose.getX() - currentPose.getX();
double translationY = targetPose.getY() - currentPose.getY();
public void goToTargetPose(Pose2d targetPose){
Pose2d currentPose = drive.getCurrentPose();
double translationX = targetPose.getX() - currentPose.getX();
double translationY = targetPose.getY() - currentPose.getY();
Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation());
Translation2d driveTranslation = new Translation2d(translationX, translationY);
double magnitude = Math.sqrt(translationX * translationX + translationY * translationY);
if (magnitude > 1.0) {
translationX /= magnitude;
translationY /= magnitude;
}
drive.driveFieldAngle(driveTranslation, deltaRotation);
}
Translation2d driveTranslation;
if (magnitude < 0.05) {
driveTranslation = new Translation2d();
} else {
driveTranslation = new Translation2d(translationX, translationY);
}
@Override
public double getError() {
return 0;
// return targetAngle - drive.getGyroAngle();
}
drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
}
@Override
public void runWithOutput(double output) {
// drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError()))));
}
@Override
public double getError() {
return 0;
}
// @Override
// public boolean isFinished() {
// Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
// double ballAngleDeg = m_lidar.getLatestBallAngleDegrees();
// // TODO: Tune
// return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5;
// }
}
@Override
public void runWithOutput(double output) {}
}