diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 547c5f2..efa01e7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -32,6 +33,7 @@ public class GrimperMur extends Command { double x; double y; double angle; + double pigeonAngle; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); Optional alliance = DriverStation.getAlliance(); @@ -59,45 +61,53 @@ public class GrimperMur extends Command { @Override public void execute() { if(limeLight3.getV()){ - BotPose = limeLight3.getBotPoseBlue(); - botx = BotPose[0]; - boty = BotPose[1]; - System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); - if(angle < 0){ - angle = angle + 360; - } - if(alliance.get() == Alliance.Blue){ - y = 5.4; - x = 1.11; - angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); + BotPose = limeLight3.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + if(angle < 0){ + angle = angle + 360; + } + if(alliance.get() == Alliance.Red){ + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + System.out.println(pigeonAngle); + y = 2.6; + x = 15.6; + angle = 180; + if(pigeonAngle< 190 && pigeonAngle> 170){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2))); + } + else{ + if(pigeonAngle>180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + } + else{ + drivetrain.setControl(drive.withRotationalRate(1)); + } + } + } + else{ + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; + System.out.println(pigeonAngle); + y = 5.4; + x = 1.11; + angle = 0; + if(pigeonAngle> 358 || pigeonAngle< 2){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); + } + else{ + if(pigeonAngle>0 && pigeonAngle<180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + System.out.println("x"); + } + else if(pigeonAngle>180){ + System.out.println("e"); + drivetrain.setControl(drive.withRotationalRate(1)); + } + } + } } else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - } - } - else{ - x = 15.6; - y = 6.959326; - angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - } - } + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); } }