Skip to content

Commit

Permalink
Merge pull request #52 from 2491-NoMythic/testingDTandLimelights
Browse files Browse the repository at this point in the history
  • Loading branch information
@veggie2u
veggie2u authored Feb 10, 2024
2 parents eef77d1 + 93f6cd4 commit 8327c56
Show file tree
Hide file tree
Showing 5 changed files with 59 additions and 25 deletions.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,6 +251,7 @@ private void configureBindings() {
new Trigger(operatorController::getSquareButton).onTrue(new ClimberPullDown(climber));
}
SmartDashboard.putData("set offsets", new InstantCommand(driveTrain::setEncoderOffsets));
SmartDashboard.putData(new InstantCommand(driveTrain::forceUpdateOdometryWithVision));

// //Intake bindings
// new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE));
Expand Down
11 changes: 10 additions & 1 deletion src/main/java/frc/robot/commands/AimRobotMoving.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,10 @@

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.settings.Constants.DriveConstants;
Expand Down Expand Up @@ -31,6 +34,7 @@ public class AimRobotMoving extends Command {
BooleanSupplier run;
DoubleSupplier rotationSupplier;
double rotationSpeed;
double allianceOffset;

public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run){
m_drivetrain = drivetrain;
Expand Down Expand Up @@ -66,11 +70,16 @@ public void execute() {
} else {
rotationSpeed = speedController.calculate(currentHeading);
}
if(DriverStation.getAlliance().get() == Alliance.Red) {
allianceOffset = Math.PI;
} else {
allianceOffset = 0;
}
m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
rotationSpeed,
m_drivetrain.getGyroscopeRotation()));
new Rotation2d(m_drivetrain.getGyroscopeRotation().getRadians()+allianceOffset)));
// m_drivetrain.drive(new ChassisSpeeds(
// translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
// translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,14 +42,14 @@ public void execute() {
drivetrain.drive(new ChassisSpeeds(
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
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND
));
} else {
drivetrain.drive(
ChassisSpeeds.fromFieldRelativeSpeeds(
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,
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND,
drivetrain.getGyroscopeRotation()
)
);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/settings/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -406,7 +406,7 @@ public final class Field{

public final class Vision{
public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-april2";
public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-april3";
public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-april";
public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural";


Expand Down
66 changes: 45 additions & 21 deletions src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,10 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) {
PathPlannerLogging.setLogActivePathCallback((poses) -> m_field.getObject("path").setPoses(poses));
SmartDashboard.putData("Field", m_field);
SmartDashboard.putData("resetOdometry", new InstantCommand(() -> this.resetOdometry()));
SmartDashboard.putBoolean("use limelight", false);
SmartDashboard.putBoolean("trust limelight", false);
SmartDashboard.putBoolean("force use limelight", false);
SmartDashboard.putBoolean("use 2 limelights", false);
modules = new SwerveModule[4];
lastAngles = new Rotation2d[] {new Rotation2d(), new Rotation2d(), new Rotation2d(), new Rotation2d()}; // manually make empty angles to avoid null errors.

Expand Down Expand Up @@ -168,7 +172,11 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) {
* 'forwards' direction.
*/
public void zeroGyroscope() {
if(DriverStation.getAlliance().get() == Alliance.Red) {
pigeon.setYaw(180);
} else {
pigeon.setYaw(0); //TODO make sure this is right for both alliances
}
odometer.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d(getPose().getTranslation(), new Rotation2d()));
}
public void zeroGyroscope(double angleDeg) {
Expand Down Expand Up @@ -331,11 +339,12 @@ public double calculateSpeakerAngleMoving(){
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);


Rotation2d unadjustedAngle = Rotation2d.fromDegrees(Math.asin(deltaX/speakerDist));
shootingTime = speakerDist/shootingSpeed; //calculates how long the note will take to reach the target
currentXSpeed = this.getChassisSpeeds().vxMetersPerSecond;
currentYSpeed = this.getChassisSpeeds().vyMetersPerSecond;
targetOffset = new Translation2d(currentXSpeed*shootingTime*OFFSET_MULTIPLIER, currentYSpeed*shootingTime*OFFSET_MULTIPLIER);
targetOffset = new Translation2d(currentXSpeed*shootingTime*OFFSET_MULTIPLIER*unadjustedAngle.getRadians(), currentYSpeed*shootingTime*OFFSET_MULTIPLIER);
//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
Expand Down Expand Up @@ -402,31 +411,46 @@ public Pose2d getAverageBotPose(LimelightValues ll2, LimelightValues ll3) {
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());}
if(SmartDashboard.getBoolean("use limelight", false) && SmartDashboard.getBoolean("use 2 limelights", false)) {
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());
updateOdometry();
AngleShooterSubsystem.setDTPose(getPose());
AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds());
if (SmartDashboard.getBoolean("use limelight", false)) {
LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME);
LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME);
Boolean isLL2VisionValid = ll2.isResultValid;
Boolean isLL3VisionValid = ll3.isResultValid;
Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition());
Boolean isLL3VisionTrustworthy = isLL3VisionValid && ll3.isPoseTrustworthy(odometer.getEstimatedPosition());
SmartDashboard.putBoolean("LL2visionValid", isLL2VisionTrustworthy);
SmartDashboard.putBoolean("LL3visionValid", isLL3VisionTrustworthy);
if (SmartDashboard.getBoolean("trust limelight", false)) {
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());}
AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds());
if(SmartDashboard.getBoolean("force use limelight", false)) {
forceUpdateOdometryWithVision();
} else {
if (SmartDashboard.getBoolean("use limelight", false)) {
if(SmartDashboard.getBoolean("use 2 limelights", false)) {
LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME);
LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME);
Boolean isLL2VisionValid = ll2.isResultValid;
Boolean isLL3VisionValid = ll3.isResultValid;
Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition());
Boolean isLL3VisionTrustworthy = isLL3VisionValid && ll3.isPoseTrustworthy(odometer.getEstimatedPosition());
SmartDashboard.putBoolean("LL2visionValid", isLL2VisionTrustworthy);
SmartDashboard.putBoolean("LL3visionValid", isLL3VisionTrustworthy);
if (SmartDashboard.getBoolean("trust limelight", false)) {
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());}
}
} else {
LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME);
Boolean isLL2VisionValid = ll2.isResultValid;
Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition());
if (isLL2VisionTrustworthy) {
updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());
}
}
}
}
m_field.setRobotPose(odometer.getEstimatedPosition());
Expand Down

0 comments on commit 8327c56

Please sign in to comment.