Skip to content

Commit

Permalink
Merge pull request #48 from 2491-NoMythic/fixingOdometry
Browse files Browse the repository at this point in the history
  • Loading branch information
@veggie2u
veggie2u authored Feb 7, 2024
2 parents 6a0cb1f + dccd2cc commit 5194b0e
Show file tree
Hide file tree
Showing 7 changed files with 84 additions and 51 deletions.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ public void teleopInit() {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotContainer.teleopInit();
}

/** This function is called periodically during operator control. */
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -321,7 +321,9 @@ private void registerNamedCommands() {
}
NamedCommands.registerCommand("wait x seconds", new WaitCommand(Preferences.getDouble("wait # of seconds", 0)));
}

public void teleopInit() {
driveTrain.forceUpdateOdometryWithVision();
}
public void teleopPeriodic() {
SmartDashboard.putData(driveTrain.getCurrentCommand());
driveTrain.calculateSpeakerAngle();
Expand Down
20 changes: 14 additions & 6 deletions src/main/java/frc/robot/commands/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
import java.util.function.DoubleSupplier;

import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.settings.Constants.DriveConstants;
import frc.robot.subsystems.DrivetrainSubsystem;
Expand All @@ -14,6 +16,7 @@ public class Drive extends Command {
private final DoubleSupplier translationXSupplier;
private final DoubleSupplier translationYSupplier;
private final DoubleSupplier rotationSupplier;
private int invert;

public Drive(DrivetrainSubsystem drivetrainSubsystem,
BooleanSupplier robotCentricMode,
Expand All @@ -30,18 +33,23 @@ public Drive(DrivetrainSubsystem drivetrainSubsystem,
@Override
public void execute() {
// You can use `new ChassisSpeeds(...)` for robot-oriented movement instead of field-oriented movement
if(DriverStation.getAlliance().get() == Alliance.Red) {
invert = -1;
} else {
invert = 1;
}
if (robotCentricMode.getAsBoolean()) {
drivetrain.drive(new ChassisSpeeds(
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert,
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert
));
} else {
drivetrain.drive(
ChassisSpeeds.fromFieldRelativeSpeeds(
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND,
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert,
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert,
drivetrain.getGyroscopeRotation()
)
);
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/settings/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -389,9 +389,9 @@ public final class PS4Operator{
}

public final class Field{
public static final double BLUE_SPEAKER_Y = 5.612;
public static final double RED_SPEAKER_Y = 2.636;
public static final double SPEAKER_X = 0.23;//16.412;
public static final double BLUE_SPEAKER_X = 0.23;
public static final double RED_SPEAKER_X = 16.49;
public static final double SPEAKER_Y = 5.612;//16.412;
public static final double SPEAKER_Z = 2.045; //height of opening
public static final double MAX_SHOOTING_DISTANCE = 2491;

Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/settings/LimelightValues.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,9 @@ public Pose2d getbotPose(){
return botPoseBlue;
}
}
public Pose2d getBotPoseBlue() {
return botPoseBlue;
}
public boolean isPoseTrustworthy(Pose2d robotPose){
Pose2d poseEstimate = this.getbotPose();
if ((poseEstimate.getX()<fieldCorner.getX() && poseEstimate.getY()<fieldCorner.getY()) //Don't trust estimations that are outside the field perimeter.
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,16 +57,16 @@ public static void setDTChassisSpeeds(ChassisSpeeds speeds) {
}

public double calculateSpeakerAngleDifference() {
double deltaY;
double deltaX;
shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS;
// triangle for robot angle
Optional<Alliance> alliance = DriverStation.getAlliance();
if (alliance.isPresent() && alliance.get() == Alliance.Red) {
deltaY = Math.abs(dtvalues.getY() - Field.RED_SPEAKER_Y);
deltaX = Math.abs(dtvalues.getX() - Field.RED_SPEAKER_X);
} else {
deltaY = Math.abs(dtvalues.getY() - Field.BLUE_SPEAKER_Y);
deltaX = Math.abs(dtvalues.getX() - Field.BLUE_SPEAKER_X);
}
double deltaX = Math.abs(dtvalues.getX() - Field.SPEAKER_X);
double deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y);
double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2));
SmartDashboard.putNumber("dist to speakre", speakerDist);

Expand All @@ -79,13 +79,13 @@ public double calculateSpeakerAngleDifference() {

// next 3 lines set where we actually want to aim, given the offset our shooting
// will have based on our speed
double offsetSpeakerX = Field.SPEAKER_X + targetOffset.getX();
double offsetSpeakerY;
double offsetSpeakerY = Field.SPEAKER_Y + targetOffset.getY();
double offsetSpeakerX;

if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) {
offsetSpeakerY = Field.RED_SPEAKER_Y + targetOffset.getY();
offsetSpeakerX = Field.RED_SPEAKER_X + targetOffset.getX();
} else {
offsetSpeakerY = Field.BLUE_SPEAKER_Y + targetOffset.getY();
offsetSpeakerX = Field.BLUE_SPEAKER_X + targetOffset.getX();
}
double offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX);
double offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY);
Expand Down
85 changes: 52 additions & 33 deletions src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -265,12 +265,16 @@ public double calculateSpeakerAngle(){
// triangle for robot angle
Optional<Alliance> alliance = DriverStation.getAlliance();
if (alliance.isPresent() && alliance.get() == Alliance.Red) {
deltaY = Math.abs(dtvalues.getY() - Field.RED_SPEAKER_Y);
deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y);
} else {
deltaY = Math.abs(dtvalues.getY() - Field.BLUE_SPEAKER_Y);
deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y);

}
deltaX = Math.abs(dtvalues.getX() - Field.SPEAKER_X);
if(DriverStation.getAlliance().get() == Alliance.Red) {
deltaX = Math.abs(dtvalues.getX() - Field.BLUE_SPEAKER_X);
} else {
deltaX = Math.abs(dtvalues.getX() - Field.RED_SPEAKER_X);
}
speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
SmartDashboard.putNumber("dist to speakre", speakerDist);
if(speakerDist<Field.MAX_SHOOTING_DISTANCE && lightsExist) {
Expand All @@ -280,8 +284,8 @@ public double calculateSpeakerAngle(){
} }

//getting desired robot angle
if (alliance.isPresent() && alliance.get() == Alliance.Blue) {
if (dtvalues.getY() >= Field.BLUE_SPEAKER_Y) {
// if (alliance.isPresent() && alliance.get() == Alliance.Blue) {
if (dtvalues.getY() >= Field.SPEAKER_Y) {
//the robot is to the left of the speaker
double thetaAbove = -Math.toDegrees(Math.asin(deltaX / speakerDist))-90;
m_desiredRobotAngle = thetaAbove;
Expand All @@ -290,18 +294,18 @@ public double calculateSpeakerAngle(){
double thetaBelow = Math.toDegrees(Math.asin(deltaX / speakerDist))+90;
m_desiredRobotAngle = thetaBelow;
}
} else {
if (dtvalues.getY() >= Field.RED_SPEAKER_Y) {
//the robot is to the left of the speaker
double thetaAbove = -Math.toDegrees(Math.asin(deltaX / speakerDist))-90;
m_desiredRobotAngle = thetaAbove;
}
else{
double thetaBelow = Math.toDegrees(Math.asin(deltaX / speakerDist))+90;
m_desiredRobotAngle = thetaBelow;
}
// } else {
// if (dtvalues.getY() >= Field.RED_SPEAKER_Y) {
// //the robot is to the left of the speaker
// double thetaAbove = -Math.toDegrees(Math.asin(deltaX / speakerDist))-90;
// m_desiredRobotAngle = thetaAbove;
// }
// else{
// double thetaBelow = Math.toDegrees(Math.asin(deltaX / speakerDist))+90;
// m_desiredRobotAngle = thetaBelow;
// }
// m_desiredRobotAngle = m_desiredRobotAngle + 180;
}
// }
SmartDashboard.putNumber("just angle", Math.toDegrees(Math.asin(deltaX / speakerDist)));
SmartDashboard.putNumber("desired angle", m_desiredRobotAngle);
return m_desiredRobotAngle;
Expand All @@ -313,11 +317,11 @@ public double calculateSpeakerAngleMoving(){
shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS;
//triangle for robot angle
if (alliance.isPresent() && alliance.get() == Alliance.Red) {
deltaY = Math.abs(dtvalues.getY() - Field.RED_SPEAKER_Y);
deltaX = Math.abs(dtvalues.getX() - Field.RED_SPEAKER_X);
} else {
deltaY = Math.abs(dtvalues.getY() - Field.BLUE_SPEAKER_Y);
deltaX = Math.abs(dtvalues.getX() - Field.BLUE_SPEAKER_X);
}
deltaX = Math.abs(dtvalues.getX() - Field.SPEAKER_X);
deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y);
speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
SmartDashboard.putNumber("dist to speakre", speakerDist);

Expand All @@ -328,12 +332,20 @@ public double calculateSpeakerAngleMoving(){
//line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime

//next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed
offsetSpeakerX = Field.SPEAKER_X+targetOffset.getX();
if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) {offsetSpeakerY = Field.RED_SPEAKER_Y+targetOffset.getY();}
else {offsetSpeakerY = Field.BLUE_SPEAKER_Y+targetOffset.getY();}
int correctionDirection;
double speakerX;
if(DriverStation.getAlliance().get() == Alliance.Blue) {
correctionDirection = 1;
speakerX = Field.BLUE_SPEAKER_X;
} else {
correctionDirection = -1;
speakerX = Field.RED_SPEAKER_X;
}
offsetSpeakerX = speakerX+(targetOffset.getX()*correctionDirection);
offsetSpeakerY = Field.SPEAKER_Y+(targetOffset.getY()*correctionDirection);
offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX);
offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY);

adjustedTarget = new Translation2d(offsetSpeakerX, offsetSpeakerY);
offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaY, 2) + Math.pow(offsetDeltaX, 2));
SmartDashboard.putNumber("offsetSpeakerDis", offsetSpeakerdist);
Expand All @@ -355,11 +367,11 @@ public double calculateSpeakerAngleMoving(){
m_desiredRobotAngle = thetaBelow;
} } else {
if (dtvalues.getY() >= adjustedTarget.getY()) {
double thetaAbove = -Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))-90;
double thetaAbove = Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))-90;
m_desiredRobotAngle = thetaAbove;
}
else{
double thetaBelow = Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))+90;
double thetaBelow = -Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))+90;
m_desiredRobotAngle = thetaBelow;
}
}
Expand All @@ -371,17 +383,24 @@ public double getSpeakerAngleDifference() {
return calculateSpeakerAngleMoving()-(getGyroscopeRotation().getDegrees()%360);
}
public Pose2d getAverageBotPose(LimelightValues ll2, LimelightValues ll3) {
double ll2X = ll2.getbotPose().getX();
double ll3X = ll3.getbotPose().getX();
double ll2Y = ll2.getbotPose().getY();
double ll3Y = ll3.getbotPose().getY();
double ll2rotation = ll2.getbotPose().getRotation().getRadians();
double ll3rotation = ll3.getbotPose().getRotation().getRadians();
double ll2X = ll2.getBotPoseBlue().getX();
double ll3X = ll3.getBotPoseBlue().getX();
double ll2Y = ll2.getBotPoseBlue().getY();
double ll3Y = ll3.getBotPoseBlue().getY();
double ll2rotation = ll2.getBotPoseBlue().getRotation().getRadians();
double ll3rotation = ll3.getBotPoseBlue().getRotation().getRadians();
Rotation2d averageRotation = new Rotation2d((ll2rotation+ll3rotation)/2);
double averageX = (ll2X+ll3X)/2;
double averageY = (ll2Y+ll3Y)/2;
return new Pose2d(new Translation2d(averageX, averageY), averageRotation);
}
public void forceUpdateOdometryWithVision() {
LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME);
LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME);
if (ll2.isResultValid && !ll3.isResultValid) {updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());}
if (!ll2.isResultValid && ll3.isResultValid) {updateOdometryWithVision(ll3.getBotPoseBlue(), ll3.gettimestamp());}
if (ll2.isResultValid && ll3.isResultValid) {updateOdometryWithVision(getAverageBotPose(ll2, ll3), ll3.gettimestamp());}
}
@Override
public void periodic() {
SmartDashboard.putString("alliance:", DriverStation.getAlliance().get().toString());
Expand All @@ -398,8 +417,8 @@ public void periodic() {
SmartDashboard.putBoolean("LL2visionValid", isLL2VisionTrustworthy);
SmartDashboard.putBoolean("LL3visionValid", isLL3VisionTrustworthy);
if (SmartDashboard.getBoolean("trust limelight", false)) {
if (isLL2VisionTrustworthy && !isLL3VisionTrustworthy) {updateOdometryWithVision(ll2.getbotPose(), ll2.gettimestamp());}
if (!isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(ll3.getbotPose(), ll3.gettimestamp());}
if (isLL2VisionTrustworthy && !isLL3VisionTrustworthy) {updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());}
if (!isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(ll3.getBotPoseBlue(), ll3.gettimestamp());}
if (isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(getAverageBotPose(ll2, ll3), ll3.gettimestamp());}
}
}
Expand Down

0 comments on commit 5194b0e

Please sign in to comment.