Skip to content

Commit

Permalink
need to rewrite auto aim
Browse files Browse the repository at this point in the history
  • Loading branch information
@PGgit08
PGgit08 committed Jan 26, 2024
1 parent f5798f2 commit 288efc6
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 35 deletions.
40 changes: 26 additions & 14 deletions src/main/java/frc/robot/commands/shooter/AutoAim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

import java.util.function.DoubleSupplier;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
Expand All @@ -24,7 +25,7 @@ public class AutoAim extends Command {
private DoubleSupplier _ySpeed;

private PIDController _headingController = new PIDController(
Constants.PID.PP_ROTATION.kP,
0.05,
0,
0
);
Expand All @@ -46,6 +47,7 @@ public AutoAim(
_ySpeed = ySpeed;

_headingController.setTolerance(2);
_headingController.enableContinuousInput(-180, 180);

addRequirements(_shooter, _vision, _swerve);
}
Expand All @@ -66,29 +68,39 @@ public void execute() {

double[] visionAngles = _vision.anglesToSpeaker();

if (_vision.isApriltagVisible() && visionAngles != null) {
desiredShooterAngle = visionAngles[1];
desiredSwerveHeading = visionAngles[0];
// if (_vision.isApriltagVisible() && visionAngles != null) {
// desiredShooterAngle = visionAngles[1];
// desiredSwerveHeading = visionAngles[0];

SmartDashboard.putNumberArray("VISION ANGLES", visionAngles);
// SmartDashboard.putNumberArray("VISION ANGLES", visionAngles);

} else {
desiredShooterAngle = _swerve.anglesToSpeaker()[1];
desiredSwerveHeading = _swerve.anglesToSpeaker()[0];
}
// } else {
// desiredShooterAngle = _swerve.anglesToSpeaker()[1];
// desiredSwerveHeading = _swerve.anglesToSpeaker()[0];
// }

desiredSwerveHeading -= currentSwerveHeading;
// desiredSwerveHeading -= currentSwerveHeading;

_shooter.setAngle(desiredShooterAngle);
// SmartDashboard.putNumber("DESIRED HEADING 360", desiredSwerveHeading);

SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading);
// desiredSwerveHeading = MathUtil.angleModulus(Math.toRadians(desiredSwerveHeading));
// desiredSwerveHeading = Math.toDegrees(desiredSwerveHeading);

// _shooter.setAngle(desiredShooterAngle);

// SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading);

desiredSwerveHeading = _swerve.anglesToSpeaker()[0];

_swerve.driveChassis(
new ChassisSpeeds(
_xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
_ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
// _headingController.calculate(currentSwerveHeading, desiredSwerveHeading),
0
MathUtil.clamp(
_headingController.calculate(currentSwerveHeading, desiredSwerveHeading),
-Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED
)
)
);
}
Expand Down
55 changes: 37 additions & 18 deletions src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
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.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
Expand Down Expand Up @@ -226,6 +227,9 @@ public void initSendable(SendableBuilder builder) {
public void periodic() {
publisher.set(states);

SmartDashboard.putNumber("Gyro 180/-180", getHeading().getDegrees());
anglesToSpeaker();

// This method will be called once per scheduler run
// SmartDashboard.putNumber("Gyro", getHeading().getDegrees());
SmartDashboard.putBoolean("Field Oriented", fieldOriented);
Expand Down Expand Up @@ -358,7 +362,7 @@ public void resetGyro() {

resetPose(new_pose);
}

/** Resets pose estimator's translation of the drive to (0, 0). */
public void resetTranslation() {
Pose2d new_pose = new Pose2d(0, 0, _pose.getRotation());
Expand Down Expand Up @@ -391,31 +395,46 @@ public Rotation2d getHeadingRaw() {

/** Get the shooter's angle to the speaker hole using the drive's pose estimator. */
public double[] anglesToSpeaker() {
int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG;
Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();
// int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG;
// Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();

double xDifference = tagPose.getX() - _pose.getX();
double yDifference = tagPose.getY() - _pose.getY();
// Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY());
// Translation2d botTranslation = _pose.getTranslation();

double distanceToRobot = Math.sqrt(Math.pow(xDifference, 2) + Math.pow(yDifference, 2));
// Rotation2d moveAngle = tagTranslation.minus(botTranslation).getAngle();

double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED;
// double[] a = {1, 2};

double angleY = Math.atan(zDifference / distanceToRobot);
// SmartDashboard.putNumber("MOVE ANGLE", moveAngle.getDegrees());

double[] a = {1, 2};

Rotation2d currentRotation = getHeading();
double angleX = 0;
return a;

if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){
angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
}
else{
angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
}
// double xDifference = tagPose.getX() - _pose.getX();
// double yDifference = tagPose.getY() - _pose.getY();

// Constants.FIELD_CONSTANTS.SPEAKER_POSE.minus(new Pose2d());

// double distanceToRobot = Math.sqrt(Math.pow(xDifference, 2) + Math.pow(yDifference, 2));

// double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED;

// double angleY = Math.atan(zDifference / distanceToRobot);


// Rotation2d currentRotation = getHeading();
// double angleX = 0;

// if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){
// angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
// }
// else{
// angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
// }

double[] angles = {angleX, angleY};
// double[] angles = {angleX, angleY};

return angles;
// return angles;
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/VisionSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ public boolean isApriltagVisible() {
public boolean isApriltagVisible(int ID) {
if (!isApriltagVisible()) return false;

return (_limelight.getTag(ID) != null);
return _limelight.getTag(ID) != null;
}

/**
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/utils/LimelightHelper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,4 @@ public JsonNode getTag(int ID) {

return null;
}


}

0 comments on commit 288efc6

Please sign in to comment.